<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.6"/>
<title>My Project: rrt_plan::rrt_planner类 参考</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { searchBox.OnSelectItem(0); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td style="padding-left: 0.5em;">
   <div id="projectname">My Project
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- 制作者 Doxygen 1.8.6 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'搜索');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>首页</span></a></li>
      <li><a href="namespaces.html"><span>命名空间</span></a></li>
      <li class="current"><a href="annotated.html"><span>类</span></a></li>
      <li><a href="files.html"><span>文件</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="搜索" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="annotated.html"><span>类列表</span></a></li>
      <li><a href="classes.html"><span>类索引</span></a></li>
      <li><a href="hierarchy.html"><span>类继承关系</span></a></li>
      <li><a href="functions.html"><span>类成员</span></a></li>
    </ul>
  </div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark">&#160;</span>全部</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark">&#160;</span>类</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark">&#160;</span>命名空间</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark">&#160;</span>文件</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark">&#160;</span>函数</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark">&#160;</span>变量</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark">&#160;</span>宏定义</a></div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="namespacerrt__plan.html">rrt_plan</a></li><li class="navelem"><a class="el" href="classrrt__plan_1_1rrt__planner.html">rrt_planner</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="summary">
<a href="#pub-methods">Public 成员函数</a> &#124;
<a href="#pub-attribs">Public 属性</a> &#124;
<a href="classrrt__plan_1_1rrt__planner-members.html">所有成员列表</a>  </div>
  <div class="headertitle">
<div class="title">rrt_plan::rrt_planner类 参考</div>  </div>
</div><!--header-->
<div class="contents">

<p><code>#include &lt;<a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a>&gt;</code></p>
<div class="dynheader">
类 rrt_plan::rrt_planner 继承关系图:</div>
<div class="dyncontent">
 <div class="center">
  <img src="classrrt__plan_1_1rrt__planner.png" usemap="#rrt_plan::rrt_planner_map" alt=""/>
  <map id="rrt_plan::rrt_planner_map" name="rrt_plan::rrt_planner_map">
</map>
 </div></div>
<table class="memberdecls">
<tr class="heading"><td colspan="2"><h2 class="groupheader"><a name="pub-methods"></a>
Public 成员函数</h2></td></tr>
<tr class="memitem:a188d2961064c51462f133f656b5595ce"><td class="memItemLeft" align="right" valign="top">&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a188d2961064c51462f133f656b5595ce">rrt_planner</a> (ros::NodeHandle &amp;)</td></tr>
<tr class="separator:a188d2961064c51462f133f656b5595ce"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:aff0490fe901b891f586fae31433f72f0"><td class="memItemLeft" align="right" valign="top">&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#aff0490fe901b891f586fae31433f72f0">rrt_planner</a> ()</td></tr>
<tr class="separator:aff0490fe901b891f586fae31433f72f0"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a0c6d148e4c72d2cd780831b7b2e7ad2c"><td class="memItemLeft" align="right" valign="top">&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a0c6d148e4c72d2cd780831b7b2e7ad2c">rrt_planner</a> (std::string name, costmap_2d::Costmap2DROS *costmap_ros)</td></tr>
<tr class="separator:a0c6d148e4c72d2cd780831b7b2e7ad2c"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:aec216e5abd94aa73b71a4db4a3e89c9d"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#aec216e5abd94aa73b71a4db4a3e89c9d">initialize</a> (std::string name, costmap_2d::Costmap2DROS *costmap_ros)</td></tr>
<tr class="separator:aec216e5abd94aa73b71a4db4a3e89c9d"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a211f1d665d81c1a340d06cdc327fadf4"><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a211f1d665d81c1a340d06cdc327fadf4">makePlan</a> (const geometry_msgs::PoseStamped &amp;start, const geometry_msgs::PoseStamped &amp;goal, std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;plan)</td></tr>
<tr class="separator:a211f1d665d81c1a340d06cdc327fadf4"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a07347499c296c693631f24dfb1cb4196"><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a07347499c296c693631f24dfb1cb4196">isCellInsideMap</a> (float x, float y)</td></tr>
<tr class="separator:a07347499c296c693631f24dfb1cb4196"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a7cbacb0aead733fc6cfde463c6ea2812"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a7cbacb0aead733fc6cfde463c6ea2812">mapToWorld</a> (double mx, double my, double &amp;wx, double &amp;wy)</td></tr>
<tr class="separator:a7cbacb0aead733fc6cfde463c6ea2812"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:aa43a4ec62844e679d9e0aefcbfa5f286"><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#aa43a4ec62844e679d9e0aefcbfa5f286">isStartAndGoalCellsValid</a> (int startCell, int goalCell)</td></tr>
<tr class="separator:aa43a4ec62844e679d9e0aefcbfa5f286"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a7c923acf8ea571bf84a2ede5a0457491"><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a7c923acf8ea571bf84a2ede5a0457491">isFree</a> (int CellID)</td></tr>
<tr class="separator:a7c923acf8ea571bf84a2ede5a0457491"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:ac6b3b3e1aa57e2dd76874064f289f370"><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#ac6b3b3e1aa57e2dd76874064f289f370">isFree</a> (int i, int j)</td></tr>
<tr class="separator:ac6b3b3e1aa57e2dd76874064f289f370"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a0a4d334b13d1946044f041ae1ddc1a41"><td class="memItemLeft" align="right" valign="top">vector&lt; int &gt;&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a0a4d334b13d1946044f041ae1ddc1a41">findFreeNeighborCell</a> (int CellID)</td></tr>
<tr class="separator:a0a4d334b13d1946044f041ae1ddc1a41"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a23e32ef1c189f5035a751047f84e052a"><td class="memItemLeft" align="right" valign="top">vector&lt; int &gt;&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a23e32ef1c189f5035a751047f84e052a">findCellPath</a> (<a class="el" href="classRRT.html">RRT</a> &amp;<a class="el" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>, int startCell, int goalCell, vector&lt; vector&lt; float &gt; &gt; gridVal)</td></tr>
<tr class="separator:a23e32ef1c189f5035a751047f84e052a"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:aac5e9f72b4c55ce4aada8d80eed92a9e"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#aac5e9f72b4c55ce4aada8d80eed92a9e">convertToPlan</a> (vector&lt; int &gt; bestPath, std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;plan, geometry_msgs::PoseStamped goal, int goalCell)</td></tr>
<tr class="separator:aac5e9f72b4c55ce4aada8d80eed92a9e"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:ac2fe5313cbc77dffddfcfe1f6605d53e"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#ac2fe5313cbc77dffddfcfe1f6605d53e">publishPlan</a> (const std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;path)</td></tr>
<tr class="separator:ac2fe5313cbc77dffddfcfe1f6605d53e"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a3071190151c4463f2e39f29c02c1f55d"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a3071190151c4463f2e39f29c02c1f55d">publishPlan2</a> (const std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;path)</td></tr>
<tr class="separator:a3071190151c4463f2e39f29c02c1f55d"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:aff6d98168c072b176287fc9c444349c6"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#aff6d98168c072b176287fc9c444349c6">getGridVal</a> (vector&lt; vector&lt; float &gt; &gt; &amp;gridVal, int startRowID, int startColID, int goalRowID, int goalColID)</td></tr>
<tr class="separator:aff6d98168c072b176287fc9c444349c6"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:aac168be892772fe17a1d841d87c2249c"><td class="memItemLeft" align="right" valign="top">std::vector<br class="typebreak"/>
&lt; geometry_msgs::PoseStamped &gt;&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#aac168be892772fe17a1d841d87c2249c">middleoptimizationLayer</a> (std::vector&lt; geometry_msgs::PoseStamped &gt; temp_transformed_plan)</td></tr>
<tr class="separator:aac168be892772fe17a1d841d87c2249c"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a980538776b19bd9610458e45991cdf0b"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a980538776b19bd9610458e45991cdf0b">getIdx</a> (int &amp;x, int &amp;y)</td></tr>
<tr class="separator:a980538776b19bd9610458e45991cdf0b"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a8f1fde39d2f3b01d7b2d11dc9482e1dc"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a8f1fde39d2f3b01d7b2d11dc9482e1dc">getCorrdinate</a> (float &amp;x, float &amp;y)</td></tr>
<tr class="separator:a8f1fde39d2f3b01d7b2d11dc9482e1dc"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a7df38e380fe80d69be29d1cea2879c59"><td class="memItemLeft" align="right" valign="top">int&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">getCellIndex</a> (int i, int j)</td></tr>
<tr class="separator:a7df38e380fe80d69be29d1cea2879c59"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:aa3c07d6b2b6bed62e4b60d3898fc80cf"><td class="memItemLeft" align="right" valign="top">int&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">getCellRowID</a> (int index)</td></tr>
<tr class="separator:aa3c07d6b2b6bed62e4b60d3898fc80cf"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a758810b2f257fe0778ed3eb1e970d99c"><td class="memItemLeft" align="right" valign="top">int&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">getCellColID</a> (int index)</td></tr>
<tr class="separator:a758810b2f257fe0778ed3eb1e970d99c"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a2647265d06a792b983b372af52cf5677"><td class="memItemLeft" align="right" valign="top">int&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a2647265d06a792b983b372af52cf5677">convertToCellIndex</a> (float x, float y)</td></tr>
<tr class="separator:a2647265d06a792b983b372af52cf5677"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:ad21e1c308a79678ca6265907bf6caa84"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#ad21e1c308a79678ca6265907bf6caa84">convertToCoordinate</a> (int index, float &amp;x, float &amp;y)</td></tr>
<tr class="separator:ad21e1c308a79678ca6265907bf6caa84"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a2b6ded4f6e7719510d94371400f097d7"><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a2b6ded4f6e7719510d94371400f097d7">checkIfOnObstacles</a> (<a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &amp;tempNode)</td></tr>
<tr class="separator:a2b6ded4f6e7719510d94371400f097d7"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:aaaab60d718aa349ef2df8dcf0402f207"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#aaaab60d718aa349ef2df8dcf0402f207">generateTempPoint</a> (<a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &amp;tempNode, int <a class="el" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>, int <a class="el" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>)</td></tr>
<tr class="separator:aaaab60d718aa349ef2df8dcf0402f207"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:ac34f8c3a590fb6f3ba04c99500a8abae"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#ac34f8c3a590fb6f3ba04c99500a8abae">generateRandNodes</a> (<a class="el" href="classRRT.html">RRT</a> &amp;<a class="el" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>, vector&lt; <a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &gt; &amp;randNodes, int <a class="el" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>, int <a class="el" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>)</td></tr>
<tr class="separator:ac34f8c3a590fb6f3ba04c99500a8abae"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a7746337004ecc5a7bdad176b5033b5bc"><td class="memItemLeft" align="right" valign="top">double&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a7746337004ecc5a7bdad176b5033b5bc">getRotateAngle</a> (double x1, double y1, double x2, double y2)</td></tr>
<tr class="separator:a7746337004ecc5a7bdad176b5033b5bc"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a22cb05cb13d3027310f3fcc2b394dda6"><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a22cb05cb13d3027310f3fcc2b394dda6">addNewNodetoRRT</a> (<a class="el" href="classRRT.html">RRT</a> &amp;<a class="el" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>, vector&lt; <a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &gt; &amp;randNodes, float goalX, float goalY, int <a class="el" href="rrt_8h.html#aaac2d108c57a5722b9d816baaed80945">rrtStepSize</a>, vector&lt; vector&lt; float &gt; &gt; gridVal)</td></tr>
<tr class="separator:a22cb05cb13d3027310f3fcc2b394dda6"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a50e32f9cd1421d374ea1aab601433b10"><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a50e32f9cd1421d374ea1aab601433b10">checkNodetoGoal</a> (int X, int Y, <a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &amp;tempNode)</td></tr>
<tr class="separator:a50e32f9cd1421d374ea1aab601433b10"><td class="memSeparator" colspan="2">&#160;</td></tr>
</table><table class="memberdecls">
<tr class="heading"><td colspan="2"><h2 class="groupheader"><a name="pub-attribs"></a>
Public 属性</h2></td></tr>
<tr class="memitem:a9547688a6e31166034dc562a0b4608b6"><td class="memItemLeft" align="right" valign="top">ros::NodeHandle&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a9547688a6e31166034dc562a0b4608b6">ROSNodeHandle</a></td></tr>
<tr class="separator:a9547688a6e31166034dc562a0b4608b6"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a92ba133c58a9ad0de8f425e856953ac1"><td class="memItemLeft" align="right" valign="top">float&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">originX</a></td></tr>
<tr class="separator:a92ba133c58a9ad0de8f425e856953ac1"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a5cfa182b92acb4e370ba19a213a0c22a"><td class="memItemLeft" align="right" valign="top">float&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">originY</a></td></tr>
<tr class="separator:a5cfa182b92acb4e370ba19a213a0c22a"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:ad23ca65f53176297ae7e9f40c7edeb69"><td class="memItemLeft" align="right" valign="top">float&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a></td></tr>
<tr class="separator:ad23ca65f53176297ae7e9f40c7edeb69"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a3dcf8908136ace532176511eb1861528"><td class="memItemLeft" align="right" valign="top">costmap_2d::Costmap2DROS *&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">costmap_ros_</a></td></tr>
<tr class="separator:a3dcf8908136ace532176511eb1861528"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a25be58fd52b495ade819f8d5785beeae"><td class="memItemLeft" align="right" valign="top">double&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a25be58fd52b495ade819f8d5785beeae">step_size_</a></td></tr>
<tr class="separator:a25be58fd52b495ade819f8d5785beeae"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:aae70d5e90c66f442c8822b573645e346"><td class="memItemLeft" align="right" valign="top">double&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#aae70d5e90c66f442c8822b573645e346">min_dist_from_robot_</a></td></tr>
<tr class="separator:aae70d5e90c66f442c8822b573645e346"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:ad6f48786945a4a394c95de25cd4c77d2"><td class="memItemLeft" align="right" valign="top">costmap_2d::Costmap2D *&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a></td></tr>
<tr class="separator:ad6f48786945a4a394c95de25cd4c77d2"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:ac1cd8655573adac056ae922a92337b8d"><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">initialized_</a></td></tr>
<tr class="separator:ac1cd8655573adac056ae922a92337b8d"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:ac74fe436b5e1fa30bda1fbe821369aa2"><td class="memItemLeft" align="right" valign="top">int&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a></td></tr>
<tr class="separator:ac74fe436b5e1fa30bda1fbe821369aa2"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a3549de56a50bff6494514878b64584df"><td class="memItemLeft" align="right" valign="top">int&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a></td></tr>
<tr class="separator:a3549de56a50bff6494514878b64584df"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a6beed3a62613c7e24813416185d12929"><td class="memItemLeft" align="right" valign="top">ros::Publisher&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a6beed3a62613c7e24813416185d12929">plan_pub_</a></td></tr>
<tr class="separator:a6beed3a62613c7e24813416185d12929"><td class="memSeparator" colspan="2">&#160;</td></tr>
<tr class="memitem:a142a2d4c28bcaa2798493accf32690ee"><td class="memItemLeft" align="right" valign="top">ros::Publisher&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="classrrt__plan_1_1rrt__planner.html#a142a2d4c28bcaa2798493accf32690ee">plan_pub_2</a></td></tr>
<tr class="separator:a142a2d4c28bcaa2798493accf32690ee"><td class="memSeparator" colspan="2">&#160;</td></tr>
</table>
<a name="details" id="details"></a><h2 class="groupheader">详细描述</h2>
<div class="textblock">
<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00076">76</a> 行定义.</p>
</div><h2 class="groupheader">构造及析构函数说明</h2>
<a class="anchor" id="a188d2961064c51462f133f656b5595ce"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">rrt_plan::rrt_planner::rrt_planner </td>
          <td>(</td>
          <td class="paramtype">ros::NodeHandle &amp;&#160;</td>
          <td class="paramname"><em>nh</em></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00064">64</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;{</div>
<div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#a9547688a6e31166034dc562a0b4608b6">ROSNodeHandle</a> = nh;</div>
<div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a9547688a6e31166034dc562a0b4608b6"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a9547688a6e31166034dc562a0b4608b6">rrt_plan::rrt_planner::ROSNodeHandle</a></div><div class="ttdeci">ros::NodeHandle ROSNodeHandle</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00084">inspire_rrt_planner.h:84</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="aff0490fe901b891f586fae31433f72f0"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">rrt_plan::rrt_planner::rrt_planner </td>
          <td>(</td>
          <td class="paramname"></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00060">60</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;                        :<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">initialized_</a>(<span class="keyword">false</span>)</div>
<div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;{</div>
<div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;</div>
<div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac1cd8655573adac056ae922a92337b8d"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">rrt_plan::rrt_planner::initialized_</a></div><div class="ttdeci">bool initialized_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00341">inspire_rrt_planner.h:341</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a0c6d148e4c72d2cd780831b7b2e7ad2c"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">rrt_plan::rrt_planner::rrt_planner </td>
          <td>(</td>
          <td class="paramtype">std::string&#160;</td>
          <td class="paramname"><em>name</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">costmap_2d::Costmap2DROS *&#160;</td>
          <td class="paramname"><em>costmap_ros</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00069">69</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;{</div>
<div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#aec216e5abd94aa73b71a4db4a3e89c9d">initialize</a>(name, costmap_ros);</div>
<div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_aec216e5abd94aa73b71a4db4a3e89c9d"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#aec216e5abd94aa73b71a4db4a3e89c9d">rrt_plan::rrt_planner::initialize</a></div><div class="ttdeci">void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00074">rrt_planner.cpp:74</a></div></div>
</div><!-- fragment -->
</div>
</div>
<h2 class="groupheader">成员函数说明</h2>
<a class="anchor" id="a22cb05cb13d3027310f3fcc2b394dda6"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">bool rrt_plan::rrt_planner::addNewNodetoRRT </td>
          <td>(</td>
          <td class="paramtype"><a class="el" href="classRRT.html">RRT</a> &amp;&#160;</td>
          <td class="paramname"><em>myRRT</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">vector&lt; <a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &gt; &amp;&#160;</td>
          <td class="paramname"><em>randNodes</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float&#160;</td>
          <td class="paramname"><em>goalX</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float&#160;</td>
          <td class="paramname"><em>goalY</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>rrtStepSize</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">vector&lt; vector&lt; float &gt; &gt;&#160;</td>
          <td class="paramname"><em>gridVal</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00235">235</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;    {</div>
<div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;        vector&lt;RRT::rrtNode&gt; nearestNodes;  <span class="comment">//nearest结点数组</span></div>
<div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;        vector&lt;RRT::rrtNode&gt; tempNodes;  <span class="comment">//临时结点数组</span></div>
<div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;        <a class="code" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> loopNode;</div>
<div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;</div>
<div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;<a class="code" href="rrt_8h.html#a962f9277eb583fb40d04a820752a9191">randNodeNum</a>; i++)</div>
<div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;        {</div>
<div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;            <span class="keywordtype">int</span> tempNearestNodeID = myRRT.<a class="code" href="classRRT.html#af35d650011a6b834fc1b2d75b879817c">getNearestNodeID</a>(randNodes[i].posX,randNodes[i].posY);</div>
<div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;            <a class="code" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> tempNearestNode=myRRT.<a class="code" href="classRRT.html#afde36442771a9552bb97572098396c2b">getNode</a>(tempNearestNodeID);</div>
<div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;            nearestNodes.push_back(tempNearestNode);</div>
<div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;        }</div>
<div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;</div>
<div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;<a class="code" href="rrt_8h.html#a962f9277eb583fb40d04a820752a9191">randNodeNum</a>; i++)</div>
<div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;        {</div>
<div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;            <span class="keywordtype">double</span> theta = atan2(randNodes[i].posY - nearestNodes[i].posY, randNodes[i].posX - nearestNodes[i].posX);<span class="comment">//两个点形成的斜率</span></div>
<div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;            loopNode.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a> = (int)(nearestNodes[i].posX + (<a class="code" href="rrt_8h.html#aaac2d108c57a5722b9d816baaed80945">rrtStepSize</a> * cos(theta)));</div>
<div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;            loopNode.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a> = (int)(nearestNodes[i].posY + (<a class="code" href="rrt_8h.html#aaac2d108c57a5722b9d816baaed80945">rrtStepSize</a> * sin(theta)));</div>
<div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;            loopNode.<a class="code" href="structRRT_1_1rrtNode.html#a9f5b9bfca61e25fe7f80d361ed7996ff">parentID</a>=nearestNodes[i].nodeID;  <span class="comment">//temp结点的父结点是它的nearest结点</span></div>
<div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;</div>
<div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;            tempNodes.push_back(loopNode);</div>
<div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;        }</div>
<div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;</div>
<div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;        vector&lt;float&gt; evaluateCost;  <span class="comment">//启发函数估价</span></div>
<div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;</div>
<div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;tempNodes.size(); i++)</div>
<div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;        {</div>
<div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;            <span class="keywordtype">int</span> tempIndex=i;</div>
<div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;            <a class="code" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> tempNode=tempNodes[i];</div>
<div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;</div>
<div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="classrrt__plan_1_1rrt__planner.html#a2b6ded4f6e7719510d94371400f097d7">checkIfOnObstacles</a>(tempNode))<span class="comment">//先检查temp结点，不在障碍物上</span></div>
<div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;            {</div>
<div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;                <span class="comment">//float costG=nearestNodes[i].depth*rrtStepSize;</span></div>
<div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;                <span class="keywordtype">float</span> costH=gridVal[tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>][tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>];  <span class="comment">//温度场作为代价值</span></div>
<div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;                evaluateCost.push_back(costH);</div>
<div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;            }</div>
<div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;            <span class="keywordflow">else</span>  <span class="comment">//tempNode结点在障碍物上</span></div>
<div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;            {</div>
<div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;                evaluateCost.push_back(<a class="code" href="rrt_8h.html#a8bb5664c6d1d0848c059844294c05988">infinity</a>);  <span class="comment">//障碍物上代价无穷大</span></div>
<div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;            }</div>
<div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;        }</div>
<div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;</div>
<div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;        <span class="keywordtype">int</span> tempIndex=-1;</div>
<div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;        <span class="keywordtype">float</span> tempCost=<a class="code" href="rrt_8h.html#a8bb5664c6d1d0848c059844294c05988">infinity</a>;</div>
<div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;evaluateCost.size(); i++)</div>
<div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;        {</div>
<div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;            <span class="keywordflow">if</span>(evaluateCost[i]&lt;tempCost)</div>
<div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;            {</div>
<div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;                tempIndex=i;</div>
<div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;                tempCost=evaluateCost[i];</div>
<div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;            }</div>
<div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;        }</div>
<div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;</div>
<div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;        <span class="keywordflow">if</span>(tempIndex!=-1)</div>
<div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160;        {</div>
<div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;            <a class="code" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> successNode=tempNodes[tempIndex];</div>
<div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;            <a class="code" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> nearestNode=nearestNodes[tempIndex];</div>
<div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;            <span class="keywordtype">float</span> costN=gridVal[nearestNode.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>][nearestNode.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>];  <span class="comment">//温度场作为代价值</span></div>
<div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;            </div>
<div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;            <span class="keywordflow">if</span>(evaluateCost[tempIndex]&gt;costN)  <span class="comment">//防止RRT随机树退化</span></div>
<div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;                <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;            </div>
<div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;            <span class="keywordtype">float</span> tempToParentDistance=<a class="code" href="rrt_8h.html#aaac2d108c57a5722b9d816baaed80945">rrtStepSize</a>;</div>
<div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;            <span class="keywordtype">bool</span> regressionFlag=<span class="keyword">true</span>;</div>
<div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;myRRT.<a class="code" href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">getTreeSize</a>(); i++)</div>
<div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;            {</div>
<div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;                <span class="keywordtype">float</span> tempDistance=sqrt(pow(myRRT.<a class="code" href="classRRT.html#afde36442771a9552bb97572098396c2b">getNode</a>(i).<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a> - successNode.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a>,2) + pow(myRRT.<a class="code" href="classRRT.html#afde36442771a9552bb97572098396c2b">getNode</a>(i).<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a> - successNode.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a>,2));</div>
<div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;</div>
<div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;                <span class="keywordflow">if</span>(tempToParentDistance-tempDistance&gt;1)</div>
<div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;                {</div>
<div class="line"><a name="l00305"></a><span class="lineno">  305</span>&#160;                    regressionFlag=<span class="keyword">false</span>;</div>
<div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;                    <span class="keywordflow">break</span>;</div>
<div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;                }</div>
<div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;            }</div>
<div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;            </div>
<div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;            <span class="keywordflow">if</span>(regressionFlag)</div>
<div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;            {</div>
<div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;                successNode.<a class="code" href="structRRT_1_1rrtNode.html#aa8adf5b50e1a2a466771ae3af56b1658">nodeID</a> = myRRT.<a class="code" href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">getTreeSize</a>();<span class="comment">//设置tempNode的id值，结点个数代表编号</span></div>
<div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;                successNode.<a class="code" href="structRRT_1_1rrtNode.html#ab20e6f5be075c8feed605c935924df30">depth</a>=nearestNodes[tempIndex].depth+1;<span class="comment">//当前结点深度等于其父结点深度加1</span></div>
<div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;</div>
<div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;                myRRT.<a class="code" href="classRRT.html#a5a0591369964c82b95f08c9f868dbd54">addNewNode</a>(successNode);</div>
<div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;</div>
<div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;                <span class="keywordflow">return</span> <span class="keyword">true</span>;</div>
<div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;            }</div>
<div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;        }</div>
<div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;</div>
<div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;    }</div>
<div class="ttc" id="rrt_8h_html_afb8b6147953aaf9e7089f5f157d5b020"><div class="ttname"><a href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a></div><div class="ttdeci">int mapReduceSize</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00023">rrt.h:23</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a9f5b9bfca61e25fe7f80d361ed7996ff"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a9f5b9bfca61e25fe7f80d361ed7996ff">RRT::rrtNode::parentID</a></div><div class="ttdeci">int parentID</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00040">rrt.h:40</a></div></div>
<div class="ttc" id="classRRT_html_afde36442771a9552bb97572098396c2b"><div class="ttname"><a href="classRRT.html#afde36442771a9552bb97572098396c2b">RRT::getNode</a></div><div class="ttdeci">rrtNode getNode(int nodeID)</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00159">rrt.h:159</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a2b6ded4f6e7719510d94371400f097d7"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a2b6ded4f6e7719510d94371400f097d7">rrt_plan::rrt_planner::checkIfOnObstacles</a></div><div class="ttdeci">bool checkIfOnObstacles(RRT::rrtNode &amp;tempNode)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00764">rrt_planner.cpp:764</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_ab20e6f5be075c8feed605c935924df30"><div class="ttname"><a href="structRRT_1_1rrtNode.html#ab20e6f5be075c8feed605c935924df30">RRT::rrtNode::depth</a></div><div class="ttdeci">int depth</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00041">rrt.h:41</a></div></div>
<div class="ttc" id="classRRT_html_a5a0591369964c82b95f08c9f868dbd54"><div class="ttname"><a href="classRRT.html#a5a0591369964c82b95f08c9f868dbd54">RRT::addNewNode</a></div><div class="ttdeci">void addNewNode(rrtNode node)</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00138">rrt.h:138</a></div></div>
<div class="ttc" id="classRRT_html_a05dc852e0b777042e290dcc96bf605bb"><div class="ttname"><a href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">RRT::getTreeSize</a></div><div class="ttdeci">int getTreeSize()</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00130">rrt.h:130</a></div></div>
<div class="ttc" id="classRRT_html_af35d650011a6b834fc1b2d75b879817c"><div class="ttname"><a href="classRRT.html#af35d650011a6b834fc1b2d75b879817c">RRT::getNearestNodeID</a></div><div class="ttdeci">int getNearestNodeID(float X, float Y)</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00170">rrt.h:170</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_aa8adf5b50e1a2a466771ae3af56b1658"><div class="ttname"><a href="structRRT_1_1rrtNode.html#aa8adf5b50e1a2a466771ae3af56b1658">RRT::rrtNode::nodeID</a></div><div class="ttdeci">int nodeID</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00037">rrt.h:37</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a219cf78a0f5689bc84744df52c37e84d"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">RRT::rrtNode::posY</a></div><div class="ttdeci">float posY</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00039">rrt.h:39</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html"><div class="ttname"><a href="structRRT_1_1rrtNode.html">RRT::rrtNode</a></div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00035">rrt.h:35</a></div></div>
<div class="ttc" id="rrt_8h_html_a962f9277eb583fb40d04a820752a9191"><div class="ttname"><a href="rrt_8h.html#a962f9277eb583fb40d04a820752a9191">randNodeNum</a></div><div class="ttdeci">int randNodeNum</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00021">rrt.h:21</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a4c2096fa1d719cf2aa49cf21916a5a4b"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">RRT::rrtNode::posX</a></div><div class="ttdeci">float posX</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00038">rrt.h:38</a></div></div>
<div class="ttc" id="rrt_8h_html_aaac2d108c57a5722b9d816baaed80945"><div class="ttname"><a href="rrt_8h.html#aaac2d108c57a5722b9d816baaed80945">rrtStepSize</a></div><div class="ttdeci">int rrtStepSize</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00020">rrt.h:20</a></div></div>
<div class="ttc" id="rrt_8h_html_a8bb5664c6d1d0848c059844294c05988"><div class="ttname"><a href="rrt_8h.html#a8bb5664c6d1d0848c059844294c05988">infinity</a></div><div class="ttdeci">float infinity</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00018">rrt.h:18</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a2b6ded4f6e7719510d94371400f097d7"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">bool rrt_plan::rrt_planner::checkIfOnObstacles </td>
          <td>(</td>
          <td class="paramtype"><a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &amp;&#160;</td>
          <td class="paramname"><em>tempNode</em></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00764">764</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00765"></a><span class="lineno">  765</span>&#160;{</div>
<div class="line"><a name="l00766"></a><span class="lineno">  766</span>&#160;    <span class="keywordflow">if</span>((tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a>&lt;<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>)&amp;&amp;(tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a>&lt;<a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>))</div>
<div class="line"><a name="l00767"></a><span class="lineno">  767</span>&#160;    {</div>
<div class="line"><a name="l00768"></a><span class="lineno">  768</span>&#160;        <span class="keywordflow">if</span>(<a class="code" href="rrt__planner_8cpp.html#ab8e6863a3519a503a24344e88545eb3d">OGM</a>[<a class="code" href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">getCellIndex</a>(tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a>,tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a>)])</div>
<div class="line"><a name="l00769"></a><span class="lineno">  769</span>&#160;            <span class="keywordflow">return</span> <span class="keyword">true</span>;</div>
<div class="line"><a name="l00770"></a><span class="lineno">  770</span>&#160;        <span class="keywordflow">else</span></div>
<div class="line"><a name="l00771"></a><span class="lineno">  771</span>&#160;            <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a name="l00772"></a><span class="lineno">  772</span>&#160;    }</div>
<div class="line"><a name="l00773"></a><span class="lineno">  773</span>&#160;    <span class="keywordflow">else</span></div>
<div class="line"><a name="l00774"></a><span class="lineno">  774</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a name="l00775"></a><span class="lineno">  775</span>&#160;}</div>
<div class="ttc" id="rrt__planner_8cpp_html_ab8e6863a3519a503a24344e88545eb3d"><div class="ttname"><a href="rrt__planner_8cpp.html#ab8e6863a3519a503a24344e88545eb3d">OGM</a></div><div class="ttdeci">bool * OGM</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00027">rrt_planner.cpp:27</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a219cf78a0f5689bc84744df52c37e84d"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">RRT::rrtNode::posY</a></div><div class="ttdeci">float posY</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00039">rrt.h:39</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a4c2096fa1d719cf2aa49cf21916a5a4b"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">RRT::rrtNode::posX</a></div><div class="ttdeci">float posX</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00038">rrt.h:38</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3549de56a50bff6494514878b64584df"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">rrt_plan::rrt_planner::height</a></div><div class="ttdeci">int height</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00343">inspire_rrt_planner.h:343</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a7df38e380fe80d69be29d1cea2879c59"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">rrt_plan::rrt_planner::getCellIndex</a></div><div class="ttdeci">int getCellIndex(int i, int j)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00120">inspire_rrt_planner.h:120</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a50e32f9cd1421d374ea1aab601433b10"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">bool rrt_plan::rrt_planner::checkNodetoGoal </td>
          <td>(</td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>X</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>Y</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype"><a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &amp;&#160;</td>
          <td class="paramname"><em>tempNode</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00324">324</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;    {</div>
<div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;        <span class="keywordtype">double</span> distance = sqrt(pow(X-tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a>,2)+pow(Y-tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a>,2));</div>
<div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;        <span class="keywordflow">if</span>(distance &lt; <a class="code" href="rrt_8h.html#aaac2d108c57a5722b9d816baaed80945">rrtStepSize</a>)  <span class="comment">//goal与nearest之间距离小于rrtStepSize时即认为到达目标点</span></div>
<div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;        {</div>
<div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;            <span class="keywordflow">return</span> <span class="keyword">true</span>;</div>
<div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;        }</div>
<div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;    }</div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a219cf78a0f5689bc84744df52c37e84d"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">RRT::rrtNode::posY</a></div><div class="ttdeci">float posY</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00039">rrt.h:39</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a4c2096fa1d719cf2aa49cf21916a5a4b"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">RRT::rrtNode::posX</a></div><div class="ttdeci">float posX</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00038">rrt.h:38</a></div></div>
<div class="ttc" id="rrt_8h_html_aaac2d108c57a5722b9d816baaed80945"><div class="ttname"><a href="rrt_8h.html#aaac2d108c57a5722b9d816baaed80945">rrtStepSize</a></div><div class="ttdeci">int rrtStepSize</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00020">rrt.h:20</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a2647265d06a792b983b372af52cf5677"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">int rrt_plan::rrt_planner::convertToCellIndex </td>
          <td>(</td>
          <td class="paramtype">float&#160;</td>
          <td class="paramname"><em>x</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float&#160;</td>
          <td class="paramname"><em>y</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00135">135</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;    {</div>
<div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;        <span class="keywordtype">int</span> cellIndex;</div>
<div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;</div>
<div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;        <span class="keywordtype">float</span> newX = x / <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>;</div>
<div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160;        <span class="keywordtype">float</span> newY = y / <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>;</div>
<div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;</div>
<div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;        cellIndex = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">getCellIndex</a>(newY, newX);</div>
<div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;</div>
<div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;        <span class="keywordflow">return</span> cellIndex;</div>
<div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;    }</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ad23ca65f53176297ae7e9f40c7edeb69"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">rrt_plan::rrt_planner::resolution</a></div><div class="ttdeci">float resolution</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00336">inspire_rrt_planner.h:336</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a7df38e380fe80d69be29d1cea2879c59"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">rrt_plan::rrt_planner::getCellIndex</a></div><div class="ttdeci">int getCellIndex(int i, int j)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00120">inspire_rrt_planner.h:120</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="ad21e1c308a79678ca6265907bf6caa84"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::convertToCoordinate </td>
          <td>(</td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>index</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float &amp;&#160;</td>
          <td class="paramname"><em>x</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float &amp;&#160;</td>
          <td class="paramname"><em>y</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00148">148</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;    {</div>
<div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;        x = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">getCellColID</a>(index) * <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>;</div>
<div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;</div>
<div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;        y = <a class="code" href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">getCellRowID</a>(index) * <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>;</div>
<div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;</div>
<div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;        x = x + <a class="code" href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">originX</a>;</div>
<div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;        y = y + <a class="code" href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">originY</a>;</div>
<div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;    }</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a5cfa182b92acb4e370ba19a213a0c22a"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">rrt_plan::rrt_planner::originY</a></div><div class="ttdeci">float originY</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00335">inspire_rrt_planner.h:335</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ad23ca65f53176297ae7e9f40c7edeb69"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">rrt_plan::rrt_planner::resolution</a></div><div class="ttdeci">float resolution</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00336">inspire_rrt_planner.h:336</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a92ba133c58a9ad0de8f425e856953ac1"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">rrt_plan::rrt_planner::originX</a></div><div class="ttdeci">float originX</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00334">inspire_rrt_planner.h:334</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a758810b2f257fe0778ed3eb1e970d99c"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">rrt_plan::rrt_planner::getCellColID</a></div><div class="ttdeci">int getCellColID(int index)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00130">inspire_rrt_planner.h:130</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_aa3c07d6b2b6bed62e4b60d3898fc80cf"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">rrt_plan::rrt_planner::getCellRowID</a></div><div class="ttdeci">int getCellRowID(int index)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00125">inspire_rrt_planner.h:125</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="aac5e9f72b4c55ce4aada8d80eed92a9e"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::convertToPlan </td>
          <td>(</td>
          <td class="paramtype">vector&lt; int &gt;&#160;</td>
          <td class="paramname"><em>bestPath</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;&#160;</td>
          <td class="paramname"><em>plan</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">geometry_msgs::PoseStamped&#160;</td>
          <td class="paramname"><em>goal</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>goalCell</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00692">692</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00693"></a><span class="lineno">  693</span>&#160;{</div>
<div class="line"><a name="l00694"></a><span class="lineno">  694</span>&#160;    <span class="comment">// convert the path</span></div>
<div class="line"><a name="l00695"></a><span class="lineno">  695</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; rrtPath.size(); i++)</div>
<div class="line"><a name="l00696"></a><span class="lineno">  696</span>&#160;    {</div>
<div class="line"><a name="l00697"></a><span class="lineno">  697</span>&#160;</div>
<div class="line"><a name="l00698"></a><span class="lineno">  698</span>&#160;        <span class="keywordtype">float</span> x = 0.0;</div>
<div class="line"><a name="l00699"></a><span class="lineno">  699</span>&#160;        <span class="keywordtype">float</span> y = 0.0;</div>
<div class="line"><a name="l00700"></a><span class="lineno">  700</span>&#160;</div>
<div class="line"><a name="l00701"></a><span class="lineno">  701</span>&#160;        <span class="keywordtype">int</span> tempNodeId=rrtPath[i];</div>
<div class="line"><a name="l00702"></a><span class="lineno">  702</span>&#160;        <span class="keywordtype">int</span> cellIndex=<a class="code" href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">getCellIndex</a>(<a class="code" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>.<a class="code" href="classRRT.html#a54760030f6b61b258b6eec4472ced906">getPosX</a>(tempNodeId),<a class="code" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>.<a class="code" href="classRRT.html#a499a6d42d47b76c306a80573cba0a555">getPosY</a>(tempNodeId));</div>
<div class="line"><a name="l00703"></a><span class="lineno">  703</span>&#160;</div>
<div class="line"><a name="l00704"></a><span class="lineno">  704</span>&#160;</div>
<div class="line"><a name="l00705"></a><span class="lineno">  705</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad21e1c308a79678ca6265907bf6caa84">convertToCoordinate</a>(cellIndex, x, y);</div>
<div class="line"><a name="l00706"></a><span class="lineno">  706</span>&#160;</div>
<div class="line"><a name="l00707"></a><span class="lineno">  707</span>&#160;        <span class="comment">//cout&lt;&lt;&quot;--cell&quot;&lt;&lt;index&lt;&lt;&quot;: x=&quot;&lt;&lt;x&lt;&lt;&quot;,y=&quot;&lt;&lt;y&lt;&lt;endl;</span></div>
<div class="line"><a name="l00708"></a><span class="lineno">  708</span>&#160;        <span class="comment">//rrtlog&lt;&lt;&quot;--cell&quot;&lt;&lt;index&lt;&lt;&quot;: x=&quot;&lt;&lt;x&lt;&lt;&quot;,y=&quot;&lt;&lt;y&lt;&lt;endl;</span></div>
<div class="line"><a name="l00709"></a><span class="lineno">  709</span>&#160;</div>
<div class="line"><a name="l00710"></a><span class="lineno">  710</span>&#160;        geometry_msgs::PoseStamped pose = goal;</div>
<div class="line"><a name="l00711"></a><span class="lineno">  711</span>&#160;</div>
<div class="line"><a name="l00712"></a><span class="lineno">  712</span>&#160;</div>
<div class="line"><a name="l00713"></a><span class="lineno">  713</span>&#160;        pose.pose.position.x = x;</div>
<div class="line"><a name="l00714"></a><span class="lineno">  714</span>&#160;        pose.pose.position.y = y;</div>
<div class="line"><a name="l00715"></a><span class="lineno">  715</span>&#160;        pose.pose.position.z = 0.0;</div>
<div class="line"><a name="l00716"></a><span class="lineno">  716</span>&#160;</div>
<div class="line"><a name="l00717"></a><span class="lineno">  717</span>&#160;        pose.pose.orientation.x = 0.0;</div>
<div class="line"><a name="l00718"></a><span class="lineno">  718</span>&#160;        pose.pose.orientation.y = 0.0;</div>
<div class="line"><a name="l00719"></a><span class="lineno">  719</span>&#160;        pose.pose.orientation.z = 0.0;</div>
<div class="line"><a name="l00720"></a><span class="lineno">  720</span>&#160;        pose.pose.orientation.w = 1.0;</div>
<div class="line"><a name="l00721"></a><span class="lineno">  721</span>&#160;</div>
<div class="line"><a name="l00722"></a><span class="lineno">  722</span>&#160;        plan.push_back(pose);</div>
<div class="line"><a name="l00723"></a><span class="lineno">  723</span>&#160;    }</div>
<div class="line"><a name="l00724"></a><span class="lineno">  724</span>&#160;</div>
<div class="line"><a name="l00725"></a><span class="lineno">  725</span>&#160;</div>
<div class="line"><a name="l00726"></a><span class="lineno">  726</span>&#160;    <span class="comment">//处理最后一个结点，即是目标点goal</span></div>
<div class="line"><a name="l00727"></a><span class="lineno">  727</span>&#160;    <span class="keywordtype">float</span> final_x = 0.0;</div>
<div class="line"><a name="l00728"></a><span class="lineno">  728</span>&#160;    <span class="keywordtype">float</span> final_y = 0.0;</div>
<div class="line"><a name="l00729"></a><span class="lineno">  729</span>&#160;    <span class="keywordtype">int</span> final_index = goalCell;</div>
<div class="line"><a name="l00730"></a><span class="lineno">  730</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad21e1c308a79678ca6265907bf6caa84">convertToCoordinate</a>(final_index, final_x, final_y);</div>
<div class="line"><a name="l00731"></a><span class="lineno">  731</span>&#160;    <span class="comment">//cout&lt;&lt;&quot;--cell&quot;&lt;&lt;final_index&lt;&lt;&quot;: final_x=&quot;&lt;&lt;final_x&lt;&lt;&quot;,final_y=&quot;&lt;&lt;final_y&lt;&lt;endl;</span></div>
<div class="line"><a name="l00732"></a><span class="lineno">  732</span>&#160;    <span class="comment">//rrtlog&lt;&lt;&quot;--cell&quot;&lt;&lt;final_index&lt;&lt;&quot;: final_x=&quot;&lt;&lt;final_x&lt;&lt;&quot;,final_y=&quot;&lt;&lt;final_y&lt;&lt;endl;</span></div>
<div class="line"><a name="l00733"></a><span class="lineno">  733</span>&#160;</div>
<div class="line"><a name="l00734"></a><span class="lineno">  734</span>&#160;    geometry_msgs::PoseStamped final_pose = goal;</div>
<div class="line"><a name="l00735"></a><span class="lineno">  735</span>&#160;    final_pose.pose.position.x = final_x;</div>
<div class="line"><a name="l00736"></a><span class="lineno">  736</span>&#160;    final_pose.pose.position.y = final_y;</div>
<div class="line"><a name="l00737"></a><span class="lineno">  737</span>&#160;    final_pose.pose.position.z = 0.0;</div>
<div class="line"><a name="l00738"></a><span class="lineno">  738</span>&#160;    final_pose.pose.orientation.x = 0.0;</div>
<div class="line"><a name="l00739"></a><span class="lineno">  739</span>&#160;    final_pose.pose.orientation.y = 0.0;</div>
<div class="line"><a name="l00740"></a><span class="lineno">  740</span>&#160;    final_pose.pose.orientation.z = 0.0;</div>
<div class="line"><a name="l00741"></a><span class="lineno">  741</span>&#160;    final_pose.pose.orientation.w = 1.0;</div>
<div class="line"><a name="l00742"></a><span class="lineno">  742</span>&#160;    plan.push_back(final_pose);  <span class="comment">//最后一个结点，即是目标点</span></div>
<div class="line"><a name="l00743"></a><span class="lineno">  743</span>&#160;</div>
<div class="line"><a name="l00744"></a><span class="lineno">  744</span>&#160;</div>
<div class="line"><a name="l00745"></a><span class="lineno">  745</span>&#160;    <span class="comment">/*</span></div>
<div class="line"><a name="l00746"></a><span class="lineno">  746</span>&#160;<span class="comment">    float path_length = 0.0;</span></div>
<div class="line"><a name="l00747"></a><span class="lineno">  747</span>&#160;<span class="comment">    std::vector&lt;geometry_msgs::PoseStamped&gt;::iterator it = plan.begin();</span></div>
<div class="line"><a name="l00748"></a><span class="lineno">  748</span>&#160;<span class="comment"></span></div>
<div class="line"><a name="l00749"></a><span class="lineno">  749</span>&#160;<span class="comment">    geometry_msgs::PoseStamped last_pose;</span></div>
<div class="line"><a name="l00750"></a><span class="lineno">  750</span>&#160;<span class="comment">    last_pose = *it;</span></div>
<div class="line"><a name="l00751"></a><span class="lineno">  751</span>&#160;<span class="comment">    it++;</span></div>
<div class="line"><a name="l00752"></a><span class="lineno">  752</span>&#160;<span class="comment">    for (; it!=plan.end(); ++it) {</span></div>
<div class="line"><a name="l00753"></a><span class="lineno">  753</span>&#160;<span class="comment">        path_length += hypot(  (*it).pose.position.x - last_pose.pose.position.x,</span></div>
<div class="line"><a name="l00754"></a><span class="lineno">  754</span>&#160;<span class="comment">                  (*it).pose.position.y - last_pose.pose.position.y );</span></div>
<div class="line"><a name="l00755"></a><span class="lineno">  755</span>&#160;<span class="comment">        last_pose = *it;</span></div>
<div class="line"><a name="l00756"></a><span class="lineno">  756</span>&#160;<span class="comment">    }</span></div>
<div class="line"><a name="l00757"></a><span class="lineno">  757</span>&#160;<span class="comment">    *</span></div>
<div class="line"><a name="l00758"></a><span class="lineno">  758</span>&#160;<span class="comment">    */</span></div>
<div class="line"><a name="l00759"></a><span class="lineno">  759</span>&#160;</div>
<div class="line"><a name="l00760"></a><span class="lineno">  760</span>&#160;    <span class="comment">//cout &lt;&lt;&quot;The global path length: &quot;&lt;&lt; path_length&lt;&lt; &quot; meters&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00761"></a><span class="lineno">  761</span>&#160;    <span class="comment">//rrtlog&lt;&lt;&quot;The global path length: &quot;&lt;&lt; path_length&lt;&lt; &quot; meters&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00762"></a><span class="lineno">  762</span>&#160;}</div>
<div class="ttc" id="rrt__planner_8cpp_html_a51725e16375c08b52f82eb2faa5a921c"><div class="ttname"><a href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a></div><div class="ttdeci">RRT myRRT</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00029">rrt_planner.cpp:29</a></div></div>
<div class="ttc" id="classRRT_html_a54760030f6b61b258b6eec4472ced906"><div class="ttname"><a href="classRRT.html#a54760030f6b61b258b6eec4472ced906">RRT::getPosX</a></div><div class="ttdeci">float getPosX(int nodeID)</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00191">rrt.h:191</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ad21e1c308a79678ca6265907bf6caa84"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ad21e1c308a79678ca6265907bf6caa84">rrt_plan::rrt_planner::convertToCoordinate</a></div><div class="ttdeci">void convertToCoordinate(int index, float &amp;x, float &amp;y)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00148">inspire_rrt_planner.h:148</a></div></div>
<div class="ttc" id="classRRT_html_a499a6d42d47b76c306a80573cba0a555"><div class="ttname"><a href="classRRT.html#a499a6d42d47b76c306a80573cba0a555">RRT::getPosY</a></div><div class="ttdeci">float getPosY(int nodeID)</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00199">rrt.h:199</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a7df38e380fe80d69be29d1cea2879c59"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">rrt_plan::rrt_planner::getCellIndex</a></div><div class="ttdeci">int getCellIndex(int i, int j)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00120">inspire_rrt_planner.h:120</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a23e32ef1c189f5035a751047f84e052a"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">vector&lt; int &gt; rrt_plan::rrt_planner::findCellPath </td>
          <td>(</td>
          <td class="paramtype"><a class="el" href="classRRT.html">RRT</a> &amp;&#160;</td>
          <td class="paramname"><em>myRRT</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>startCell</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>goalCell</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">vector&lt; vector&lt; float &gt; &gt;&#160;</td>
          <td class="paramname"><em>gridVal</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00651">651</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00652"></a><span class="lineno">  652</span>&#160;{</div>
<div class="line"><a name="l00653"></a><span class="lineno">  653</span>&#160;    vector&lt;int&gt;  rrtPath;</div>
<div class="line"><a name="l00654"></a><span class="lineno">  654</span>&#160;</div>
<div class="line"><a name="l00655"></a><span class="lineno">  655</span>&#160;    vector&lt;RRT::rrtNode&gt; randNodes;</div>
<div class="line"><a name="l00656"></a><span class="lineno">  656</span>&#160;</div>
<div class="line"><a name="l00657"></a><span class="lineno">  657</span>&#160;    <span class="keywordtype">bool</span> addNodeResult = <span class="keyword">false</span>, nodeToGoal = <span class="keyword">false</span>;</div>
<div class="line"><a name="l00658"></a><span class="lineno">  658</span>&#160;</div>
<div class="line"><a name="l00659"></a><span class="lineno">  659</span>&#160;    <span class="keywordflow">while</span>(1)</div>
<div class="line"><a name="l00660"></a><span class="lineno">  660</span>&#160;    {</div>
<div class="line"><a name="l00661"></a><span class="lineno">  661</span>&#160;        randNodes.clear();</div>
<div class="line"><a name="l00662"></a><span class="lineno">  662</span>&#160;        <span class="comment">//generateTempPoint(tempNode,width, height);  //一次产生一个随机结点</span></div>
<div class="line"><a name="l00663"></a><span class="lineno">  663</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#ac34f8c3a590fb6f3ba04c99500a8abae">generateRandNodes</a>( myRRT,randNodes,<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>, <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>);  <span class="comment">//一次产生一个随机结点数组</span></div>
<div class="line"><a name="l00664"></a><span class="lineno">  664</span>&#160;</div>
<div class="line"><a name="l00665"></a><span class="lineno">  665</span>&#160;        <span class="comment">//addNodeResult = addNewPointtoRRT(myRRT,tempNode,rrtStepSize);  //经典RRT方法添加新结点</span></div>
<div class="line"><a name="l00666"></a><span class="lineno">  666</span>&#160;        addNodeResult = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a22cb05cb13d3027310f3fcc2b394dda6">addNewNodetoRRT</a>(myRRT,randNodes,<a class="code" href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">getCellRowID</a>(goalCell), <a class="code" href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">getCellColID</a>(goalCell),<a class="code" href="rrt_8h.html#aaac2d108c57a5722b9d816baaed80945">rrtStepSize</a>,gridVal); <span class="comment">//启发函数优化RRT方法</span></div>
<div class="line"><a name="l00667"></a><span class="lineno">  667</span>&#160;</div>
<div class="line"><a name="l00668"></a><span class="lineno">  668</span>&#160;        <span class="keywordflow">if</span>(addNodeResult)</div>
<div class="line"><a name="l00669"></a><span class="lineno">  669</span>&#160;        {</div>
<div class="line"><a name="l00670"></a><span class="lineno">  670</span>&#160;            <span class="comment">//nodeToGoal = checkNodetoGoal(getCellRowID(goalCell), getCellColID(goalCell),tempNode);//经典RRT方法添加新结点</span></div>
<div class="line"><a name="l00671"></a><span class="lineno">  671</span>&#160;</div>
<div class="line"><a name="l00672"></a><span class="lineno">  672</span>&#160;            <a class="code" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> lastNode=myRRT.<a class="code" href="classRRT.html#afde36442771a9552bb97572098396c2b">getNode</a>(myRRT.<a class="code" href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">getTreeSize</a>()-1);</div>
<div class="line"><a name="l00673"></a><span class="lineno">  673</span>&#160;            nodeToGoal = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a50e32f9cd1421d374ea1aab601433b10">checkNodetoGoal</a>(<a class="code" href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">getCellRowID</a>(goalCell), <a class="code" href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">getCellColID</a>(goalCell),lastNode);<span class="comment">//启发函数优化RRT方法</span></div>
<div class="line"><a name="l00674"></a><span class="lineno">  674</span>&#160;</div>
<div class="line"><a name="l00675"></a><span class="lineno">  675</span>&#160;            <span class="keywordflow">if</span>(nodeToGoal)</div>
<div class="line"><a name="l00676"></a><span class="lineno">  676</span>&#160;            {</div>
<div class="line"><a name="l00677"></a><span class="lineno">  677</span>&#160;                <span class="comment">//path = myRRT.getRootToEndPath(tempNode.nodeID);//经典RRT方法得到路径</span></div>
<div class="line"><a name="l00678"></a><span class="lineno">  678</span>&#160;</div>
<div class="line"><a name="l00679"></a><span class="lineno">  679</span>&#160;                rrtPath = myRRT.<a class="code" href="classRRT.html#a487dc3036806a9fb506a718257d8a846">getRootToEndPath</a>(myRRT.<a class="code" href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">getTreeSize</a>()-1);<span class="comment">//启发函数优化RRT方法得到路径</span></div>
<div class="line"><a name="l00680"></a><span class="lineno">  680</span>&#160;</div>
<div class="line"><a name="l00681"></a><span class="lineno">  681</span>&#160;                <span class="keywordflow">break</span>;</div>
<div class="line"><a name="l00682"></a><span class="lineno">  682</span>&#160;            }</div>
<div class="line"><a name="l00683"></a><span class="lineno">  683</span>&#160;        }</div>
<div class="line"><a name="l00684"></a><span class="lineno">  684</span>&#160;    }</div>
<div class="line"><a name="l00685"></a><span class="lineno">  685</span>&#160;</div>
<div class="line"><a name="l00686"></a><span class="lineno">  686</span>&#160;    cout&lt;&lt;<span class="stringliteral">&quot;RRT nodeSum=&quot;</span>&lt;&lt;myRRT.<a class="code" href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">getTreeSize</a>()&lt;&lt;endl;</div>
<div class="line"><a name="l00687"></a><span class="lineno">  687</span>&#160;    <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;RRT nodeSum=&quot;</span>&lt;&lt;myRRT.<a class="code" href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">getTreeSize</a>()&lt;&lt;endl;</div>
<div class="line"><a name="l00688"></a><span class="lineno">  688</span>&#160;</div>
<div class="line"><a name="l00689"></a><span class="lineno">  689</span>&#160;    <span class="keywordflow">return</span> rrtPath;</div>
<div class="line"><a name="l00690"></a><span class="lineno">  690</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a22cb05cb13d3027310f3fcc2b394dda6"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a22cb05cb13d3027310f3fcc2b394dda6">rrt_plan::rrt_planner::addNewNodetoRRT</a></div><div class="ttdeci">bool addNewNodetoRRT(RRT &amp;myRRT, vector&lt; RRT::rrtNode &gt; &amp;randNodes, float goalX, float goalY, int rrtStepSize, vector&lt; vector&lt; float &gt; &gt; gridVal)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00235">inspire_rrt_planner.h:235</a></div></div>
<div class="ttc" id="classRRT_html_afde36442771a9552bb97572098396c2b"><div class="ttname"><a href="classRRT.html#afde36442771a9552bb97572098396c2b">RRT::getNode</a></div><div class="ttdeci">rrtNode getNode(int nodeID)</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00159">rrt.h:159</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
<div class="ttc" id="classRRT_html_a05dc852e0b777042e290dcc96bf605bb"><div class="ttname"><a href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">RRT::getTreeSize</a></div><div class="ttdeci">int getTreeSize()</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00130">rrt.h:130</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac34f8c3a590fb6f3ba04c99500a8abae"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac34f8c3a590fb6f3ba04c99500a8abae">rrt_plan::rrt_planner::generateRandNodes</a></div><div class="ttdeci">void generateRandNodes(RRT &amp;myRRT, vector&lt; RRT::rrtNode &gt; &amp;randNodes, int width, int height)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00168">inspire_rrt_planner.h:168</a></div></div>
<div class="ttc" id="rrt__planner_8cpp_html_ae63d6003ff9416f7c0be6892e5bb899c"><div class="ttname"><a href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a></div><div class="ttdeci">ofstream irrtlog(&quot;/home/xuefengchang/irrtlog.txt&quot;)</div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a50e32f9cd1421d374ea1aab601433b10"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a50e32f9cd1421d374ea1aab601433b10">rrt_plan::rrt_planner::checkNodetoGoal</a></div><div class="ttdeci">bool checkNodetoGoal(int X, int Y, RRT::rrtNode &amp;tempNode)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00324">inspire_rrt_planner.h:324</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html"><div class="ttname"><a href="structRRT_1_1rrtNode.html">RRT::rrtNode</a></div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00035">rrt.h:35</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a758810b2f257fe0778ed3eb1e970d99c"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">rrt_plan::rrt_planner::getCellColID</a></div><div class="ttdeci">int getCellColID(int index)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00130">inspire_rrt_planner.h:130</a></div></div>
<div class="ttc" id="classRRT_html_a487dc3036806a9fb506a718257d8a846"><div class="ttname"><a href="classRRT.html#a487dc3036806a9fb506a718257d8a846">RRT::getRootToEndPath</a></div><div class="ttdeci">vector&lt; int &gt; getRootToEndPath(int endNodeID)</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00273">rrt.h:273</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3549de56a50bff6494514878b64584df"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">rrt_plan::rrt_planner::height</a></div><div class="ttdeci">int height</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00343">inspire_rrt_planner.h:343</a></div></div>
<div class="ttc" id="rrt_8h_html_aaac2d108c57a5722b9d816baaed80945"><div class="ttname"><a href="rrt_8h.html#aaac2d108c57a5722b9d816baaed80945">rrtStepSize</a></div><div class="ttdeci">int rrtStepSize</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00020">rrt.h:20</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_aa3c07d6b2b6bed62e4b60d3898fc80cf"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">rrt_plan::rrt_planner::getCellRowID</a></div><div class="ttdeci">int getCellRowID(int index)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00125">inspire_rrt_planner.h:125</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a0a4d334b13d1946044f041ae1ddc1a41"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">vector&lt; int &gt; rrt_plan::rrt_planner::findFreeNeighborCell </td>
          <td>(</td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>CellID</em></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00809">809</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00810"></a><span class="lineno">  810</span>&#160;{</div>
<div class="line"><a name="l00811"></a><span class="lineno">  811</span>&#160;</div>
<div class="line"><a name="l00812"></a><span class="lineno">  812</span>&#160;    <span class="keywordtype">int</span> rowID=<a class="code" href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">getCellRowID</a>(CellID);</div>
<div class="line"><a name="l00813"></a><span class="lineno">  813</span>&#160;    <span class="keywordtype">int</span> colID=<a class="code" href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">getCellColID</a>(CellID);</div>
<div class="line"><a name="l00814"></a><span class="lineno">  814</span>&#160;    <span class="keywordtype">int</span> neighborIndex;</div>
<div class="line"><a name="l00815"></a><span class="lineno">  815</span>&#160;    vector &lt;int&gt;  freeNeighborCells;</div>
<div class="line"><a name="l00816"></a><span class="lineno">  816</span>&#160;</div>
<div class="line"><a name="l00817"></a><span class="lineno">  817</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=-1; i&lt;=1; i++)</div>
<div class="line"><a name="l00818"></a><span class="lineno">  818</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> j=-1; j&lt;=1; j++)</div>
<div class="line"><a name="l00819"></a><span class="lineno">  819</span>&#160;        {</div>
<div class="line"><a name="l00820"></a><span class="lineno">  820</span>&#160;            <span class="comment">//check whether the index is valid</span></div>
<div class="line"><a name="l00821"></a><span class="lineno">  821</span>&#160;            <span class="keywordflow">if</span> ((rowID+i&gt;=0)&amp;&amp;(rowID+i&lt;<a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>)&amp;&amp;(colID+j&gt;=0)&amp;&amp;(colID+j&lt;<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>)&amp;&amp; (!(i==0 &amp;&amp; j==0)))</div>
<div class="line"><a name="l00822"></a><span class="lineno">  822</span>&#160;            {</div>
<div class="line"><a name="l00823"></a><span class="lineno">  823</span>&#160;                neighborIndex = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">getCellIndex</a>(rowID+i,colID+j);</div>
<div class="line"><a name="l00824"></a><span class="lineno">  824</span>&#160;                <span class="keywordflow">if</span>(<a class="code" href="classrrt__plan_1_1rrt__planner.html#a7c923acf8ea571bf84a2ede5a0457491">isFree</a>(neighborIndex) )</div>
<div class="line"><a name="l00825"></a><span class="lineno">  825</span>&#160;                    freeNeighborCells.push_back(neighborIndex);</div>
<div class="line"><a name="l00826"></a><span class="lineno">  826</span>&#160;            }</div>
<div class="line"><a name="l00827"></a><span class="lineno">  827</span>&#160;        }</div>
<div class="line"><a name="l00828"></a><span class="lineno">  828</span>&#160;    <span class="keywordflow">return</span>  freeNeighborCells;</div>
<div class="line"><a name="l00829"></a><span class="lineno">  829</span>&#160;</div>
<div class="line"><a name="l00830"></a><span class="lineno">  830</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a7c923acf8ea571bf84a2ede5a0457491"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a7c923acf8ea571bf84a2ede5a0457491">rrt_plan::rrt_planner::isFree</a></div><div class="ttdeci">bool isFree(int CellID)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00804">rrt_planner.cpp:804</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a758810b2f257fe0778ed3eb1e970d99c"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">rrt_plan::rrt_planner::getCellColID</a></div><div class="ttdeci">int getCellColID(int index)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00130">inspire_rrt_planner.h:130</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3549de56a50bff6494514878b64584df"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">rrt_plan::rrt_planner::height</a></div><div class="ttdeci">int height</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00343">inspire_rrt_planner.h:343</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_aa3c07d6b2b6bed62e4b60d3898fc80cf"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">rrt_plan::rrt_planner::getCellRowID</a></div><div class="ttdeci">int getCellRowID(int index)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00125">inspire_rrt_planner.h:125</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a7df38e380fe80d69be29d1cea2879c59"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">rrt_plan::rrt_planner::getCellIndex</a></div><div class="ttdeci">int getCellIndex(int i, int j)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00120">inspire_rrt_planner.h:120</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="ac34f8c3a590fb6f3ba04c99500a8abae"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::generateRandNodes </td>
          <td>(</td>
          <td class="paramtype"><a class="el" href="classRRT.html">RRT</a> &amp;&#160;</td>
          <td class="paramname"><em>myRRT</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">vector&lt; <a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &gt; &amp;&#160;</td>
          <td class="paramname"><em>randNodes</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>width</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>height</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00168">168</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;    {</div>
<div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;        <a class="code" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> loopNode;</div>
<div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;</div>
<div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;<a class="code" href="rrt_8h.html#a962f9277eb583fb40d04a820752a9191">randNodeNum</a>; i++)</div>
<div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;        {</div>
<div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;            <span class="keywordtype">int</span> x = rand() % <a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a> + 1;  <span class="comment">//行</span></div>
<div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;            <span class="keywordtype">int</span> y = rand() % <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a> + 1;  <span class="comment">//列</span></div>
<div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;          </div>
<div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;            loopNode.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a> = x;</div>
<div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;            loopNode.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a> = y;</div>
<div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;</div>
<div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;            randNodes.push_back(loopNode);</div>
<div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;        }</div>
<div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;</div>
<div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;    }</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a219cf78a0f5689bc84744df52c37e84d"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">RRT::rrtNode::posY</a></div><div class="ttdeci">float posY</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00039">rrt.h:39</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html"><div class="ttname"><a href="structRRT_1_1rrtNode.html">RRT::rrtNode</a></div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00035">rrt.h:35</a></div></div>
<div class="ttc" id="rrt_8h_html_a962f9277eb583fb40d04a820752a9191"><div class="ttname"><a href="rrt_8h.html#a962f9277eb583fb40d04a820752a9191">randNodeNum</a></div><div class="ttdeci">int randNodeNum</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00021">rrt.h:21</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a4c2096fa1d719cf2aa49cf21916a5a4b"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">RRT::rrtNode::posX</a></div><div class="ttdeci">float posX</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00038">rrt.h:38</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3549de56a50bff6494514878b64584df"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">rrt_plan::rrt_planner::height</a></div><div class="ttdeci">int height</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00343">inspire_rrt_planner.h:343</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="aaaab60d718aa349ef2df8dcf0402f207"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::generateTempPoint </td>
          <td>(</td>
          <td class="paramtype"><a class="el" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a> &amp;&#160;</td>
          <td class="paramname"><em>tempNode</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>width</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>height</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00160">160</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;    {</div>
<div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;        <span class="keywordtype">int</span> x = rand() % <a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a> + 1;  <span class="comment">//行</span></div>
<div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;        <span class="keywordtype">int</span> y = rand() % <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a> + 1;  <span class="comment">//列</span></div>
<div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;        tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a> = x;</div>
<div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;        tempNode.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a> = y;</div>
<div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;    }</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a219cf78a0f5689bc84744df52c37e84d"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">RRT::rrtNode::posY</a></div><div class="ttdeci">float posY</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00039">rrt.h:39</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a4c2096fa1d719cf2aa49cf21916a5a4b"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">RRT::rrtNode::posX</a></div><div class="ttdeci">float posX</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00038">rrt.h:38</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3549de56a50bff6494514878b64584df"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">rrt_plan::rrt_planner::height</a></div><div class="ttdeci">int height</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00343">inspire_rrt_planner.h:343</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a758810b2f257fe0778ed3eb1e970d99c"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">int rrt_plan::rrt_planner::getCellColID </td>
          <td>(</td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>index</em></td><td>)</td>
          <td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00130">130</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;    {</div>
<div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;        <span class="keywordflow">return</span> index%<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>;</div>
<div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;    }</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a7df38e380fe80d69be29d1cea2879c59"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">int rrt_plan::rrt_planner::getCellIndex </td>
          <td>(</td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>i</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>j</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00120">120</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;    {</div>
<div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;        <span class="keywordflow">return</span> (i*<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>)+j;</div>
<div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;    }</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="aa3c07d6b2b6bed62e4b60d3898fc80cf"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">int rrt_plan::rrt_planner::getCellRowID </td>
          <td>(</td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>index</em></td><td>)</td>
          <td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00125">125</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;    {</div>
<div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;        <span class="keywordflow">return</span> index/<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>;</div>
<div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;    }</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a8f1fde39d2f3b01d7b2d11dc9482e1dc"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::getCorrdinate </td>
          <td>(</td>
          <td class="paramtype">float &amp;&#160;</td>
          <td class="paramname"><em>x</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float &amp;&#160;</td>
          <td class="paramname"><em>y</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00114">114</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;    {</div>
<div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;        x = x - <a class="code" href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">originX</a>;</div>
<div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;        y = y - <a class="code" href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">originY</a>;</div>
<div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;    }</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a5cfa182b92acb4e370ba19a213a0c22a"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">rrt_plan::rrt_planner::originY</a></div><div class="ttdeci">float originY</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00335">inspire_rrt_planner.h:335</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a92ba133c58a9ad0de8f425e856953ac1"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">rrt_plan::rrt_planner::originX</a></div><div class="ttdeci">float originX</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00334">inspire_rrt_planner.h:334</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="aff6d98168c072b176287fc9c444349c6"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::getGridVal </td>
          <td>(</td>
          <td class="paramtype">vector&lt; vector&lt; float &gt; &gt; &amp;&#160;</td>
          <td class="paramname"><em>gridVal</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>startRowID</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>startColID</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>goalRowID</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>goalColID</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00240">240</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;{</div>
<div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;    <span class="keywordtype">int</span> subwidth=<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>,subheight=<a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>;</div>
<div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;    <span class="keywordtype">int</span> i , j , m , n , s;</div>
<div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;    <span class="keywordtype">float</span> gridMap[subheight][subwidth];</div>
<div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;</div>
<div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;<span class="preprocessor">    #pragma opm parallel for private(i,j) schedule(static)</span></div>
<div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;<span class="preprocessor"></span>    <span class="keywordflow">for</span> (i=0; i&lt;subheight; i++)</div>
<div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;    {</div>
<div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;        <span class="keywordflow">for</span> (j=0; j&lt;subwidth; j++)</div>
<div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;        {</div>
<div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;            gridMap[i][j] = 1.0f;  <span class="comment">//1表示自由区域</span></div>
<div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;</div>
<div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;            gridVal[i][j] = 0.0f;</div>
<div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;            <span class="keywordflow">for</span> (m=0; m&lt;<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>; m++)</div>
<div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;            {</div>
<div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;                <span class="keywordflow">for</span> (n=0; n&lt;<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>; n++)</div>
<div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;                {</div>
<div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;                    <span class="keywordtype">int</span> cellIndex=<a class="code" href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">getCellIndex</a>(i*mapReduceSize+m,j*mapReduceSize+n);</div>
<div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;                    <span class="keywordflow">if</span>(!<a class="code" href="rrt__planner_8cpp.html#a56d338f4195d0ebc4906a1085f3eed83">OGM2</a>[cellIndex])</div>
<div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;                    {</div>
<div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;                        gridMap[i][j] = 0.0f;  <span class="comment">//0表示障碍物</span></div>
<div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;                    }</div>
<div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;                }</div>
<div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;            }</div>
<div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;        }</div>
<div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;    }</div>
<div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;</div>
<div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;    </div>
<div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;    <span class="comment">//irrtlog&lt;&lt;&quot;#############################&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;    <span class="comment">//for(int k=0; k&lt;subheight; k++)</span></div>
<div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;    <span class="comment">//{</span></div>
<div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;     <span class="comment">//   for(int l=0; l&lt;subwidth; l++)</span></div>
<div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;     <span class="comment">//   {</span></div>
<div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;     <span class="comment">//       irrtlog&lt;&lt;gridMap[k][l]&lt;&lt;&quot;,&quot;;</span></div>
<div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;    <span class="comment">//    }</span></div>
<div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;    <span class="comment">//    irrtlog&lt;&lt;endl;</span></div>
<div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;    <span class="comment">//}</span></div>
<div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;    <span class="comment">//irrtlog&lt;&lt;&quot;$$$$$$$$$$$$$$$$$$$$$$$$$$$$$&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;    </div>
<div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;</div>
<div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;    <span class="keywordtype">int</span> initIdx[2] = {0}; <span class="comment">// index of initial point</span></div>
<div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;    <span class="keywordtype">int</span> goalIdx[2] = {0}; <span class="comment">// index of goal point</span></div>
<div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;</div>
<div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#a980538776b19bd9610458e45991cdf0b">getIdx</a>(startRowID, startColID);</div>
<div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#a980538776b19bd9610458e45991cdf0b">getIdx</a>(goalRowID, goalColID);</div>
<div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;</div>
<div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;    initIdx[0]=startRowID;</div>
<div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;    initIdx[1]=startColID;</div>
<div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160;    goalIdx[0]=goalRowID;</div>
<div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;    goalIdx[1]=goalColID;</div>
<div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;</div>
<div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;    gridVal[initIdx[0]][initIdx[1]] = <a class="code" href="rrt_8h.html#ad255730b84e5f3fabaf7cff0f5b835e2">initialTemperature</a>;</div>
<div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;    gridVal[goalIdx[0]][goalIdx[1]] = -<a class="code" href="rrt_8h.html#ad255730b84e5f3fabaf7cff0f5b835e2">initialTemperature</a>;</div>
<div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;    </div>
<div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;    </div>
<div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;<span class="preprocessor">    #pragma omp parallel for private(s,i,j) schedule(static)</span></div>
<div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;<span class="preprocessor"></span>    <span class="keywordflow">for</span> (s=0; s&lt;<a class="code" href="rrt_8h.html#ab205788f3d8cd37826d8ae7bd4675cee">iterationTime</a>; s++)</div>
<div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;    {</div>
<div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;        {</div>
<div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;            <span class="keywordflow">for</span> (i=1; i&lt;subheight-1; i++)</div>
<div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;            {</div>
<div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;                <span class="keywordflow">for</span> (j=1; j&lt;subwidth-1; j++)</div>
<div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;                {</div>
<div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;                    <span class="keywordflow">if</span> (gridMap[i][j])</div>
<div class="line"><a name="l00305"></a><span class="lineno">  305</span>&#160;                    {</div>
<div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;                        <span class="keywordtype">float</span> delta = (gridVal[i-1][j]*gridMap[i-1][j]</div>
<div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;                                       +gridVal[i][j-1]*gridMap[i][j-1]</div>
<div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;                                       +gridVal[i+1][j]*gridMap[i+1][j]</div>
<div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;                                       +gridVal[i][j+1]*gridMap[i][j+1])</div>
<div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;                                      /(gridMap[i-1][j]+gridMap[i][j-1]+gridMap[i+1][j]+gridMap[i][j+1]);</div>
<div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;                        gridVal[i][j] = delta;</div>
<div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;                    }</div>
<div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;                }</div>
<div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;            }</div>
<div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;        }</div>
<div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;    }</div>
<div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;}</div>
<div class="ttc" id="rrt_8h_html_afb8b6147953aaf9e7089f5f157d5b020"><div class="ttname"><a href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a></div><div class="ttdeci">int mapReduceSize</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00023">rrt.h:23</a></div></div>
<div class="ttc" id="rrt__planner_8cpp_html_a56d338f4195d0ebc4906a1085f3eed83"><div class="ttname"><a href="rrt__planner_8cpp.html#a56d338f4195d0ebc4906a1085f3eed83">OGM2</a></div><div class="ttdeci">bool * OGM2</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00028">rrt_planner.cpp:28</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
<div class="ttc" id="rrt_8h_html_ab205788f3d8cd37826d8ae7bd4675cee"><div class="ttname"><a href="rrt_8h.html#ab205788f3d8cd37826d8ae7bd4675cee">iterationTime</a></div><div class="ttdeci">int iterationTime</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00024">rrt.h:24</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a980538776b19bd9610458e45991cdf0b"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a980538776b19bd9610458e45991cdf0b">rrt_plan::rrt_planner::getIdx</a></div><div class="ttdeci">void getIdx(int &amp;x, int &amp;y)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00108">inspire_rrt_planner.h:108</a></div></div>
<div class="ttc" id="rrt_8h_html_ad255730b84e5f3fabaf7cff0f5b835e2"><div class="ttname"><a href="rrt_8h.html#ad255730b84e5f3fabaf7cff0f5b835e2">initialTemperature</a></div><div class="ttdeci">float initialTemperature</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00025">rrt.h:25</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3549de56a50bff6494514878b64584df"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">rrt_plan::rrt_planner::height</a></div><div class="ttdeci">int height</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00343">inspire_rrt_planner.h:343</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a7df38e380fe80d69be29d1cea2879c59"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">rrt_plan::rrt_planner::getCellIndex</a></div><div class="ttdeci">int getCellIndex(int i, int j)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00120">inspire_rrt_planner.h:120</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a980538776b19bd9610458e45991cdf0b"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::getIdx </td>
          <td>(</td>
          <td class="paramtype">int &amp;&#160;</td>
          <td class="paramname"><em>x</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int &amp;&#160;</td>
          <td class="paramname"><em>y</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00108">108</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;    {</div>
<div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;        x = x/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>;</div>
<div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;        y = y/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>;</div>
<div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;    }</div>
<div class="ttc" id="rrt_8h_html_afb8b6147953aaf9e7089f5f157d5b020"><div class="ttname"><a href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a></div><div class="ttdeci">int mapReduceSize</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00023">rrt.h:23</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a7746337004ecc5a7bdad176b5033b5bc"></a>
<div class="memitem">
<div class="memproto">
<table class="mlabels">
  <tr>
  <td class="mlabels-left">
      <table class="memname">
        <tr>
          <td class="memname">double rrt_plan::rrt_planner::getRotateAngle </td>
          <td>(</td>
          <td class="paramtype">double&#160;</td>
          <td class="paramname"><em>x1</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">double&#160;</td>
          <td class="paramname"><em>y1</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">double&#160;</td>
          <td class="paramname"><em>x2</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">double&#160;</td>
          <td class="paramname"><em>y2</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
  </td>
  <td class="mlabels-right">
<span class="mlabels"><span class="mlabel">inline</span></span>  </td>
  </tr>
</table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00197">197</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;    {</div>
<div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;        <span class="keyword">const</span> <span class="keywordtype">double</span> epsilon = 1.0e-6;</div>
<div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;        <span class="keyword">const</span> <span class="keywordtype">double</span> nyPI = acos(-1.0);</div>
<div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;        <span class="keywordtype">double</span> dist, dot, degree, angle;</div>
<div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;</div>
<div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;        <span class="comment">// normalize</span></div>
<div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;        dist = sqrt( x1 * x1 + y1 * y1 );</div>
<div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;        x1 /= dist;</div>
<div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;        y1 /= dist;</div>
<div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;        dist = sqrt( x2 * x2 + y2 * y2 );</div>
<div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;        x2 /= dist;</div>
<div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;        y2 /= dist;</div>
<div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;        <span class="comment">// dot product</span></div>
<div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;        dot = x1 * x2 + y1 * y2;</div>
<div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;        <span class="keywordflow">if</span> ( fabs(dot-1.0) &lt;= epsilon )</div>
<div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;            angle = 0.0;</div>
<div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;        <span class="keywordflow">else</span> <span class="keywordflow">if</span> ( fabs(dot+1.0) &lt;= epsilon )</div>
<div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;            angle = nyPI;</div>
<div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;        <span class="keywordflow">else</span></div>
<div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;        {</div>
<div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;            <span class="keywordtype">double</span> cross;</div>
<div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;</div>
<div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;            angle = acos(dot);</div>
<div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;            <span class="comment">//cross product</span></div>
<div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;            cross = x1 * y2 - x2 * y1;</div>
<div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;            <span class="comment">// vector p2 is clockwise from vector p1</span></div>
<div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;            <span class="comment">// with respect to the origin (0.0)</span></div>
<div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;            <span class="keywordflow">if</span> (cross &lt; 0 )</div>
<div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;            {</div>
<div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;                angle = 2 * nyPI - angle;</div>
<div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;            }</div>
<div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;        }</div>
<div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;        <span class="comment">//degree = angle *  180.0 / nyPI;</span></div>
<div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;        <span class="comment">//return degree;</span></div>
<div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;        <span class="keywordflow">return</span> angle;</div>
<div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;    }</div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="aec216e5abd94aa73b71a4db4a3e89c9d"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::initialize </td>
          <td>(</td>
          <td class="paramtype">std::string&#160;</td>
          <td class="paramname"><em>name</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">costmap_2d::Costmap2DROS *&#160;</td>
          <td class="paramname"><em>costmap_ros</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">
<p>overriden classes from interface nav_core::BaseGlobalPlanner </p>

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00074">74</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;{</div>
<div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;</div>
<div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;    <span class="keywordflow">if</span> (!<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">initialized_</a>)</div>
<div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;    {</div>
<div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">costmap_ros_</a> = costmap_ros;</div>
<div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a> = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">costmap_ros_</a>-&gt;getCostmap();</div>
<div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;</div>
<div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;        ros::NodeHandle private_nh(<span class="stringliteral">&quot;~/&quot;</span> + name);</div>
<div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;</div>
<div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">originX</a> = <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getOriginX();</div>
<div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">originY</a> = <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getOriginY();</div>
<div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;</div>
<div class="line"><a name="l00087"></a><span class="lineno">   87</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a> = <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getSizeInCellsX();</div>
<div class="line"><a name="l00088"></a><span class="lineno">   88</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a> = <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getSizeInCellsY();</div>
<div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a> = <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getResolution();</div>
<div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160;        <a class="code" href="rrt__planner_8cpp.html#a298f4debf9e9db57dcaa736916e4b451">mapSize</a> = <a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>*<a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>;</div>
<div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;</div>
<div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;        ROS_INFO(<span class="stringliteral">&quot;---originX=%f,originY=%f---&quot;</span>,<a class="code" href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">originX</a>,<a class="code" href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">originY</a>);</div>
<div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;</div>
<div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;        ROS_INFO(<span class="stringliteral">&quot;---width=%d,height=%d---&quot;</span>,<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>,height);</div>
<div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;</div>
<div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;        ROS_INFO(<span class="stringliteral">&quot;---resolution=%f---&quot;</span>,<a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>);</div>
<div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;</div>
<div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#a6beed3a62613c7e24813416185d12929">plan_pub_</a> = private_nh.advertise&lt;nav_msgs::Path&gt;(<span class="stringliteral">&quot;plan&quot;</span>, 1);</div>
<div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#a142a2d4c28bcaa2798493accf32690ee">plan_pub_2</a> = private_nh.advertise&lt;nav_msgs::Path&gt;(<span class="stringliteral">&quot;plan2&quot;</span>, 1);</div>
<div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;</div>
<div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;        <a class="code" href="rrt__planner_8cpp.html#ab8e6863a3519a503a24344e88545eb3d">OGM</a> = <span class="keyword">new</span> <span class="keywordtype">bool</span> [<a class="code" href="rrt__planner_8cpp.html#a298f4debf9e9db57dcaa736916e4b451">mapSize</a>];</div>
<div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> iy = 0; iy &lt; <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getSizeInCellsY(); iy++)</div>
<div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;        {</div>
<div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;            <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> ix = 0; ix &lt; <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getSizeInCellsX(); ix++)</div>
<div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;            {</div>
<div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;                <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> cost = <span class="keyword">static_cast&lt;</span><span class="keywordtype">int</span><span class="keyword">&gt;</span>(<a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getCost(ix, iy));</div>
<div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;</div>
<div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;                <span class="comment">//if (cost == 0)</span></div>
<div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;                <span class="keywordflow">if</span>(cost&lt;=127)  <span class="comment">// 调节避障条件，范围越大，要求越松散</span></div>
<div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;                    OGM[iy*<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>+ix]=<span class="keyword">true</span>;</div>
<div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;                <span class="keywordflow">else</span></div>
<div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;                    OGM[iy*<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>+ix]=<span class="keyword">false</span>;</div>
<div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;            }</div>
<div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;        }</div>
<div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;</div>
<div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;        <a class="code" href="rrt__planner_8cpp.html#a56d338f4195d0ebc4906a1085f3eed83">OGM2</a> = <span class="keyword">new</span> <span class="keywordtype">bool</span> [<a class="code" href="rrt__planner_8cpp.html#a298f4debf9e9db57dcaa736916e4b451">mapSize</a>];</div>
<div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> iy = 0; iy &lt; <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getSizeInCellsY(); iy++)</div>
<div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;        {</div>
<div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;            <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> ix = 0; ix &lt; <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getSizeInCellsX(); ix++)</div>
<div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;            {</div>
<div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;                <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> cost = <span class="keyword">static_cast&lt;</span><span class="keywordtype">int</span><span class="keyword">&gt;</span>(<a class="code" href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">costmap_</a>-&gt;getCost(ix, iy));</div>
<div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;</div>
<div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;                <span class="keywordflow">if</span>(cost==254)  <span class="comment">// 调节避障条件，范围越大，要求越松散</span></div>
<div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;                    <a class="code" href="rrt__planner_8cpp.html#a56d338f4195d0ebc4906a1085f3eed83">OGM2</a>[iy*<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>+ix]=<span class="keyword">false</span>;</div>
<div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;                <span class="keywordflow">else</span></div>
<div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;                    <a class="code" href="rrt__planner_8cpp.html#a56d338f4195d0ebc4906a1085f3eed83">OGM2</a>[iy*<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>+ix]=<span class="keyword">true</span>;</div>
<div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;            }</div>
<div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;        }</div>
<div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;        </div>
<div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;        ROS_INFO(<span class="stringliteral">&quot;---rrt planner1 initialized successfully---&quot;</span>);</div>
<div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;</div>
<div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">initialized_</a> = <span class="keyword">true</span>;</div>
<div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;    }</div>
<div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;    <span class="keywordflow">else</span></div>
<div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;    {</div>
<div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;        ROS_WARN(<span class="stringliteral">&quot;---This planner1 has already been initialized... doing nothing---&quot;</span>);</div>
<div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;    }</div>
<div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;}</div>
<div class="ttc" id="rrt__planner_8cpp_html_a298f4debf9e9db57dcaa736916e4b451"><div class="ttname"><a href="rrt__planner_8cpp.html#a298f4debf9e9db57dcaa736916e4b451">mapSize</a></div><div class="ttdeci">int mapSize</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00026">rrt_planner.cpp:26</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a6beed3a62613c7e24813416185d12929"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a6beed3a62613c7e24813416185d12929">rrt_plan::rrt_planner::plan_pub_</a></div><div class="ttdeci">ros::Publisher plan_pub_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00345">inspire_rrt_planner.h:345</a></div></div>
<div class="ttc" id="rrt__planner_8cpp_html_ab8e6863a3519a503a24344e88545eb3d"><div class="ttname"><a href="rrt__planner_8cpp.html#ab8e6863a3519a503a24344e88545eb3d">OGM</a></div><div class="ttdeci">bool * OGM</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00027">rrt_planner.cpp:27</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3dcf8908136ace532176511eb1861528"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">rrt_plan::rrt_planner::costmap_ros_</a></div><div class="ttdeci">costmap_2d::Costmap2DROS * costmap_ros_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00337">inspire_rrt_planner.h:337</a></div></div>
<div class="ttc" id="rrt__planner_8cpp_html_a56d338f4195d0ebc4906a1085f3eed83"><div class="ttname"><a href="rrt__planner_8cpp.html#a56d338f4195d0ebc4906a1085f3eed83">OGM2</a></div><div class="ttdeci">bool * OGM2</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00028">rrt_planner.cpp:28</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a5cfa182b92acb4e370ba19a213a0c22a"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">rrt_plan::rrt_planner::originY</a></div><div class="ttdeci">float originY</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00335">inspire_rrt_planner.h:335</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ad23ca65f53176297ae7e9f40c7edeb69"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">rrt_plan::rrt_planner::resolution</a></div><div class="ttdeci">float resolution</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00336">inspire_rrt_planner.h:336</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a92ba133c58a9ad0de8f425e856953ac1"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">rrt_plan::rrt_planner::originX</a></div><div class="ttdeci">float originX</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00334">inspire_rrt_planner.h:334</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac1cd8655573adac056ae922a92337b8d"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">rrt_plan::rrt_planner::initialized_</a></div><div class="ttdeci">bool initialized_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00341">inspire_rrt_planner.h:341</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3549de56a50bff6494514878b64584df"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">rrt_plan::rrt_planner::height</a></div><div class="ttdeci">int height</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00343">inspire_rrt_planner.h:343</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a142a2d4c28bcaa2798493accf32690ee"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a142a2d4c28bcaa2798493accf32690ee">rrt_plan::rrt_planner::plan_pub_2</a></div><div class="ttdeci">ros::Publisher plan_pub_2</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00346">inspire_rrt_planner.h:346</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ad6f48786945a4a394c95de25cd4c77d2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ad6f48786945a4a394c95de25cd4c77d2">rrt_plan::rrt_planner::costmap_</a></div><div class="ttdeci">costmap_2d::Costmap2D * costmap_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00339">inspire_rrt_planner.h:339</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a07347499c296c693631f24dfb1cb4196"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">bool rrt_plan::rrt_planner::isCellInsideMap </td>
          <td>(</td>
          <td class="paramtype">float&#160;</td>
          <td class="paramname"><em>x</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float&#160;</td>
          <td class="paramname"><em>y</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00778">778</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00779"></a><span class="lineno">  779</span>&#160;{</div>
<div class="line"><a name="l00780"></a><span class="lineno">  780</span>&#160;    <span class="keywordtype">bool</span> valid = <span class="keyword">true</span>;</div>
<div class="line"><a name="l00781"></a><span class="lineno">  781</span>&#160;</div>
<div class="line"><a name="l00782"></a><span class="lineno">  782</span>&#160;    <span class="keywordflow">if</span> (x &gt; (<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a> * <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>) || y &gt; (<a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a> * resolution))</div>
<div class="line"><a name="l00783"></a><span class="lineno">  783</span>&#160;        valid = <span class="keyword">false</span>;</div>
<div class="line"><a name="l00784"></a><span class="lineno">  784</span>&#160;</div>
<div class="line"><a name="l00785"></a><span class="lineno">  785</span>&#160;    <span class="keywordflow">return</span> valid;</div>
<div class="line"><a name="l00786"></a><span class="lineno">  786</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ad23ca65f53176297ae7e9f40c7edeb69"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">rrt_plan::rrt_planner::resolution</a></div><div class="ttdeci">float resolution</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00336">inspire_rrt_planner.h:336</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3549de56a50bff6494514878b64584df"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">rrt_plan::rrt_planner::height</a></div><div class="ttdeci">int height</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00343">inspire_rrt_planner.h:343</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a7c923acf8ea571bf84a2ede5a0457491"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">bool rrt_plan::rrt_planner::isFree </td>
          <td>(</td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>CellID</em></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00804">804</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00805"></a><span class="lineno">  805</span>&#160;{</div>
<div class="line"><a name="l00806"></a><span class="lineno">  806</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="rrt__planner_8cpp.html#ab8e6863a3519a503a24344e88545eb3d">OGM</a>[CellID];</div>
<div class="line"><a name="l00807"></a><span class="lineno">  807</span>&#160;}</div>
<div class="ttc" id="rrt__planner_8cpp_html_ab8e6863a3519a503a24344e88545eb3d"><div class="ttname"><a href="rrt__planner_8cpp.html#ab8e6863a3519a503a24344e88545eb3d">OGM</a></div><div class="ttdeci">bool * OGM</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00027">rrt_planner.cpp:27</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="ac6b3b3e1aa57e2dd76874064f289f370"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">bool rrt_plan::rrt_planner::isFree </td>
          <td>(</td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>i</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>j</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00796">796</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00797"></a><span class="lineno">  797</span>&#160;{</div>
<div class="line"><a name="l00798"></a><span class="lineno">  798</span>&#160;    <span class="keywordtype">int</span> CellID = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">getCellIndex</a>(i, j);</div>
<div class="line"><a name="l00799"></a><span class="lineno">  799</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="rrt__planner_8cpp.html#ab8e6863a3519a503a24344e88545eb3d">OGM</a>[CellID];</div>
<div class="line"><a name="l00800"></a><span class="lineno">  800</span>&#160;</div>
<div class="line"><a name="l00801"></a><span class="lineno">  801</span>&#160;}</div>
<div class="ttc" id="rrt__planner_8cpp_html_ab8e6863a3519a503a24344e88545eb3d"><div class="ttname"><a href="rrt__planner_8cpp.html#ab8e6863a3519a503a24344e88545eb3d">OGM</a></div><div class="ttdeci">bool * OGM</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00027">rrt_planner.cpp:27</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a7df38e380fe80d69be29d1cea2879c59"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a7df38e380fe80d69be29d1cea2879c59">rrt_plan::rrt_planner::getCellIndex</a></div><div class="ttdeci">int getCellIndex(int i, int j)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00120">inspire_rrt_planner.h:120</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="aa43a4ec62844e679d9e0aefcbfa5f286"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">bool rrt_plan::rrt_planner::isStartAndGoalCellsValid </td>
          <td>(</td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>startCell</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">int&#160;</td>
          <td class="paramname"><em>goalCell</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00833">833</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00834"></a><span class="lineno">  834</span>&#160;{</div>
<div class="line"><a name="l00835"></a><span class="lineno">  835</span>&#160;    <span class="keywordtype">bool</span> isvalid=<span class="keyword">true</span>;</div>
<div class="line"><a name="l00836"></a><span class="lineno">  836</span>&#160;    <span class="keywordtype">bool</span> isFreeStartCell=<a class="code" href="classrrt__plan_1_1rrt__planner.html#a7c923acf8ea571bf84a2ede5a0457491">isFree</a>(startCell);</div>
<div class="line"><a name="l00837"></a><span class="lineno">  837</span>&#160;    <span class="keywordtype">bool</span> isFreeGoalCell=<a class="code" href="classrrt__plan_1_1rrt__planner.html#a7c923acf8ea571bf84a2ede5a0457491">isFree</a>(goalCell);</div>
<div class="line"><a name="l00838"></a><span class="lineno">  838</span>&#160;    <span class="keywordflow">if</span> (startCell==goalCell)</div>
<div class="line"><a name="l00839"></a><span class="lineno">  839</span>&#160;    {</div>
<div class="line"><a name="l00840"></a><span class="lineno">  840</span>&#160;        cout &lt;&lt; <span class="stringliteral">&quot;------The Start and the Goal cells are the same...------&quot;</span> &lt;&lt; endl;</div>
<div class="line"><a name="l00841"></a><span class="lineno">  841</span>&#160;        isvalid = <span class="keyword">false</span>;</div>
<div class="line"><a name="l00842"></a><span class="lineno">  842</span>&#160;    }</div>
<div class="line"><a name="l00843"></a><span class="lineno">  843</span>&#160;    <span class="keywordflow">else</span></div>
<div class="line"><a name="l00844"></a><span class="lineno">  844</span>&#160;    {</div>
<div class="line"><a name="l00845"></a><span class="lineno">  845</span>&#160;        isvalid=(isFreeStartCell&amp;&amp;isFreeGoalCell);</div>
<div class="line"><a name="l00846"></a><span class="lineno">  846</span>&#160;    }</div>
<div class="line"><a name="l00847"></a><span class="lineno">  847</span>&#160;    <span class="keywordflow">return</span> isvalid;</div>
<div class="line"><a name="l00848"></a><span class="lineno">  848</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a7c923acf8ea571bf84a2ede5a0457491"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a7c923acf8ea571bf84a2ede5a0457491">rrt_plan::rrt_planner::isFree</a></div><div class="ttdeci">bool isFree(int CellID)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00804">rrt_planner.cpp:804</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a211f1d665d81c1a340d06cdc327fadf4"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">bool rrt_plan::rrt_planner::makePlan </td>
          <td>(</td>
          <td class="paramtype">const geometry_msgs::PoseStamped &amp;&#160;</td>
          <td class="paramname"><em>start</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">const geometry_msgs::PoseStamped &amp;&#160;</td>
          <td class="paramname"><em>goal</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;&#160;</td>
          <td class="paramname"><em>plan</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00382">382</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00384"></a><span class="lineno">  384</span>&#160;{</div>
<div class="line"><a name="l00385"></a><span class="lineno">  385</span>&#160;    srand((<span class="keywordtype">unsigned</span>)time(NULL));<span class="comment">//设置随机数种子</span></div>
<div class="line"><a name="l00386"></a><span class="lineno">  386</span>&#160;</div>
<div class="line"><a name="l00387"></a><span class="lineno">  387</span>&#160;    <span class="keywordflow">if</span> (!<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">initialized_</a>)</div>
<div class="line"><a name="l00388"></a><span class="lineno">  388</span>&#160;    {</div>
<div class="line"><a name="l00389"></a><span class="lineno">  389</span>&#160;        ROS_ERROR(<span class="stringliteral">&quot;---The planner has not been initialized, please call initialize() to use the planner---&quot;</span>);</div>
<div class="line"><a name="l00390"></a><span class="lineno">  390</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;    }</div>
<div class="line"><a name="l00392"></a><span class="lineno">  392</span>&#160;</div>
<div class="line"><a name="l00393"></a><span class="lineno">  393</span>&#160;    <span class="keywordflow">if</span> (goal.header.frame_id != <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">costmap_ros_</a>-&gt;getGlobalFrameID())</div>
<div class="line"><a name="l00394"></a><span class="lineno">  394</span>&#160;    {</div>
<div class="line"><a name="l00395"></a><span class="lineno">  395</span>&#160;        ROS_ERROR(<span class="stringliteral">&quot;---This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.---&quot;</span>,</div>
<div class="line"><a name="l00396"></a><span class="lineno">  396</span>&#160;                  <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">costmap_ros_</a>-&gt;getGlobalFrameID().c_str(), goal.header.frame_id.c_str());</div>
<div class="line"><a name="l00397"></a><span class="lineno">  397</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a name="l00398"></a><span class="lineno">  398</span>&#160;    }</div>
<div class="line"><a name="l00399"></a><span class="lineno">  399</span>&#160;</div>
<div class="line"><a name="l00400"></a><span class="lineno">  400</span>&#160;</div>
<div class="line"><a name="l00401"></a><span class="lineno">  401</span>&#160;    plan.clear();</div>
<div class="line"><a name="l00402"></a><span class="lineno">  402</span>&#160;</div>
<div class="line"><a name="l00403"></a><span class="lineno">  403</span>&#160;    tf::Stamped &lt; tf::Pose &gt; goal_tf;</div>
<div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;    tf::Stamped &lt; tf::Pose &gt; start_tf;</div>
<div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;</div>
<div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;    poseStampedMsgToTF(goal, goal_tf);</div>
<div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;    poseStampedMsgToTF(start, start_tf);</div>
<div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;</div>
<div class="line"><a name="l00409"></a><span class="lineno">  409</span>&#160;    <span class="comment">// convert the start and goal positions</span></div>
<div class="line"><a name="l00410"></a><span class="lineno">  410</span>&#160;</div>
<div class="line"><a name="l00411"></a><span class="lineno">  411</span>&#160;    <span class="keywordtype">float</span> startX = start.pose.position.x;</div>
<div class="line"><a name="l00412"></a><span class="lineno">  412</span>&#160;    <span class="keywordtype">float</span> startY = start.pose.position.y;</div>
<div class="line"><a name="l00413"></a><span class="lineno">  413</span>&#160;</div>
<div class="line"><a name="l00414"></a><span class="lineno">  414</span>&#160;    <span class="keywordtype">float</span> goalX = goal.pose.position.x;</div>
<div class="line"><a name="l00415"></a><span class="lineno">  415</span>&#160;    <span class="keywordtype">float</span> goalY = goal.pose.position.y;</div>
<div class="line"><a name="l00416"></a><span class="lineno">  416</span>&#160;</div>
<div class="line"><a name="l00417"></a><span class="lineno">  417</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#a8f1fde39d2f3b01d7b2d11dc9482e1dc">getCorrdinate</a>(startX, startY);</div>
<div class="line"><a name="l00418"></a><span class="lineno">  418</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#a8f1fde39d2f3b01d7b2d11dc9482e1dc">getCorrdinate</a>(goalX, goalY);</div>
<div class="line"><a name="l00419"></a><span class="lineno">  419</span>&#160;</div>
<div class="line"><a name="l00420"></a><span class="lineno">  420</span>&#160;    <span class="keywordtype">int</span> startCell;</div>
<div class="line"><a name="l00421"></a><span class="lineno">  421</span>&#160;    <span class="keywordtype">int</span> goalCell;</div>
<div class="line"><a name="l00422"></a><span class="lineno">  422</span>&#160;</div>
<div class="line"><a name="l00423"></a><span class="lineno">  423</span>&#160;    <span class="comment">//ROS_INFO(&quot;---startX=%f,startY=%f---&quot;,startX,startY);</span></div>
<div class="line"><a name="l00424"></a><span class="lineno">  424</span>&#160;    <span class="comment">//ROS_INFO(&quot;---goalX=%f,goalY=%f---&quot;,goalX,goalY);</span></div>
<div class="line"><a name="l00425"></a><span class="lineno">  425</span>&#160;</div>
<div class="line"><a name="l00426"></a><span class="lineno">  426</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classrrt__plan_1_1rrt__planner.html#a07347499c296c693631f24dfb1cb4196">isCellInsideMap</a>(startX, startY) &amp;&amp; <a class="code" href="classrrt__plan_1_1rrt__planner.html#a07347499c296c693631f24dfb1cb4196">isCellInsideMap</a>(goalX, goalY))</div>
<div class="line"><a name="l00427"></a><span class="lineno">  427</span>&#160;    {</div>
<div class="line"><a name="l00428"></a><span class="lineno">  428</span>&#160;        startCell = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a2647265d06a792b983b372af52cf5677">convertToCellIndex</a>(startX, startY);</div>
<div class="line"><a name="l00429"></a><span class="lineno">  429</span>&#160;        goalCell = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a2647265d06a792b983b372af52cf5677">convertToCellIndex</a>(goalX, goalY);</div>
<div class="line"><a name="l00430"></a><span class="lineno">  430</span>&#160;        <span class="comment">//cout &lt;&lt; startCell &lt;&lt;&quot;\t&quot;&lt;&lt; start.pose.position.x &lt;&lt;&quot;\t&quot;&lt;&lt; start.pose.position.y &lt;&lt;&quot;\t&quot;</span></div>
<div class="line"><a name="l00431"></a><span class="lineno">  431</span>&#160;        <span class="comment">//    &lt;&lt; goalCell &lt;&lt;&quot;\t&quot;&lt;&lt; goal.pose.position.x &lt;&lt;&quot;\t&quot;&lt;&lt; goal.pose.position.y&lt;&lt;&quot;\n&quot;;</span></div>
<div class="line"><a name="l00432"></a><span class="lineno">  432</span>&#160;    }</div>
<div class="line"><a name="l00433"></a><span class="lineno">  433</span>&#160;    <span class="keywordflow">else</span></div>
<div class="line"><a name="l00434"></a><span class="lineno">  434</span>&#160;    {</div>
<div class="line"><a name="l00435"></a><span class="lineno">  435</span>&#160;        ROS_WARN(<span class="stringliteral">&quot;---the start or goal is out of the map---&quot;</span>);</div>
<div class="line"><a name="l00436"></a><span class="lineno">  436</span>&#160;        <span class="comment">//rrtlog&lt;&lt;&quot;---the start or goal is out of the map---&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00437"></a><span class="lineno">  437</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a name="l00438"></a><span class="lineno">  438</span>&#160;    }</div>
<div class="line"><a name="l00439"></a><span class="lineno">  439</span>&#160;</div>
<div class="line"><a name="l00440"></a><span class="lineno">  440</span>&#160;    <span class="comment">//cout&lt;&lt;&quot;startposion:&quot;&lt;&lt;getCellRowID(startCell)&lt;&lt;&quot;  &quot;&lt;&lt;getCellColID(startCell)&lt;&lt;endl;</span></div>
<div class="line"><a name="l00441"></a><span class="lineno">  441</span>&#160;    <span class="comment">//cout&lt;&lt;&quot;goalposion:&quot;&lt;&lt;getCellRowID(goalCell)&lt;&lt;&quot;  &quot;&lt;&lt;getCellColID(goalCell)&lt;&lt;endl;</span></div>
<div class="line"><a name="l00442"></a><span class="lineno">  442</span>&#160;</div>
<div class="line"><a name="l00443"></a><span class="lineno">  443</span>&#160;    vector&lt;int&gt; rrtPath;</div>
<div class="line"><a name="l00444"></a><span class="lineno">  444</span>&#160;</div>
<div class="line"><a name="l00445"></a><span class="lineno">  445</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classrrt__plan_1_1rrt__planner.html#aa43a4ec62844e679d9e0aefcbfa5f286">isStartAndGoalCellsValid</a>(startCell, goalCell))</div>
<div class="line"><a name="l00446"></a><span class="lineno">  446</span>&#160;    {</div>
<div class="line"><a name="l00447"></a><span class="lineno">  447</span>&#160;     </div>
<div class="line"><a name="l00448"></a><span class="lineno">  448</span>&#160;            <span class="keywordtype">long</span> t1,t2,t3;</div>
<div class="line"><a name="l00449"></a><span class="lineno">  449</span>&#160;            t1=<a class="code" href="rrt__planner_8cpp.html#a3b34910310800875174c4846e64a61bd">getCurrentTime</a>();</div>
<div class="line"><a name="l00450"></a><span class="lineno">  450</span>&#160;            <span class="comment">//timespec time1, time2, time3;</span></div>
<div class="line"><a name="l00451"></a><span class="lineno">  451</span>&#160;            <span class="comment">/* take current time here */</span></div>
<div class="line"><a name="l00452"></a><span class="lineno">  452</span>&#160;            <span class="comment">//clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &amp;time1);</span></div>
<div class="line"><a name="l00453"></a><span class="lineno">  453</span>&#160;</div>
<div class="line"><a name="l00454"></a><span class="lineno">  454</span>&#160;            vector&lt;vector&lt;float&gt; &gt; gridVal;</div>
<div class="line"><a name="l00455"></a><span class="lineno">  455</span>&#160;            gridVal.clear();</div>
<div class="line"><a name="l00456"></a><span class="lineno">  456</span>&#160;</div>
<div class="line"><a name="l00457"></a><span class="lineno">  457</span>&#160;            gridVal.resize(<a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>);</div>
<div class="line"><a name="l00458"></a><span class="lineno">  458</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;(<a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>); i++)</div>
<div class="line"><a name="l00459"></a><span class="lineno">  459</span>&#160;                gridVal[i].resize(<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>);</div>
<div class="line"><a name="l00460"></a><span class="lineno">  460</span>&#160;</div>
<div class="line"><a name="l00461"></a><span class="lineno">  461</span>&#160;            <a class="code" href="classrrt__plan_1_1rrt__planner.html#aff6d98168c072b176287fc9c444349c6">getGridVal</a>(gridVal,<a class="code" href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">getCellRowID</a>(startCell), <a class="code" href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">getCellColID</a>(startCell),<a class="code" href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">getCellRowID</a>(goalCell), <a class="code" href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">getCellColID</a>(goalCell));</div>
<div class="line"><a name="l00462"></a><span class="lineno">  462</span>&#160;            <span class="comment">//clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &amp;time2);</span></div>
<div class="line"><a name="l00463"></a><span class="lineno">  463</span>&#160;             t2=<a class="code" href="rrt__planner_8cpp.html#a3b34910310800875174c4846e64a61bd">getCurrentTime</a>();</div>
<div class="line"><a name="l00464"></a><span class="lineno">  464</span>&#160;            </div>
<div class="line"><a name="l00465"></a><span class="lineno">  465</span>&#160;            </div>
<div class="line"><a name="l00466"></a><span class="lineno">  466</span>&#160;            <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;+++++++++++++++++++++++++++++++&quot;</span>&lt;&lt;endl;</div>
<div class="line"><a name="l00467"></a><span class="lineno">  467</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> k=0; k&lt;(<a class="code" href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">height</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>); k++)</div>
<div class="line"><a name="l00468"></a><span class="lineno">  468</span>&#160;            {</div>
<div class="line"><a name="l00469"></a><span class="lineno">  469</span>&#160;                <span class="keywordflow">for</span>(<span class="keywordtype">int</span> l=0; l&lt;(<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>-1); l++)</div>
<div class="line"><a name="l00470"></a><span class="lineno">  470</span>&#160;                {</div>
<div class="line"><a name="l00471"></a><span class="lineno">  471</span>&#160;                    <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;gridVal[k][l]&lt;&lt;<span class="stringliteral">&quot; &quot;</span>;</div>
<div class="line"><a name="l00472"></a><span class="lineno">  472</span>&#160;                }</div>
<div class="line"><a name="l00473"></a><span class="lineno">  473</span>&#160;                <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;gridVal[k][<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">width</a>/<a class="code" href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a>]&lt;&lt;endl;</div>
<div class="line"><a name="l00474"></a><span class="lineno">  474</span>&#160;                <span class="comment">//irrtlog&lt;&lt;endl;</span></div>
<div class="line"><a name="l00475"></a><span class="lineno">  475</span>&#160;            }</div>
<div class="line"><a name="l00476"></a><span class="lineno">  476</span>&#160;            <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;-------------------------------&quot;</span>&lt;&lt;endl;</div>
<div class="line"><a name="l00477"></a><span class="lineno">  477</span>&#160;            </div>
<div class="line"><a name="l00478"></a><span class="lineno">  478</span>&#160;            </div>
<div class="line"><a name="l00479"></a><span class="lineno">  479</span>&#160;</div>
<div class="line"><a name="l00480"></a><span class="lineno">  480</span>&#160;            <a class="code" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>.<a class="code" href="classRRT.html#affd1f4f4e35541cdca748bac5c1bbd34">clearRRT</a>();</div>
<div class="line"><a name="l00481"></a><span class="lineno">  481</span>&#160;            <a class="code" href="classRRT.html">RRT</a> temprrt(<a class="code" href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">getCellRowID</a>(startCell),<a class="code" href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">getCellColID</a>(startCell));</div>
<div class="line"><a name="l00482"></a><span class="lineno">  482</span>&#160;            <a class="code" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>=temprrt;</div>
<div class="line"><a name="l00483"></a><span class="lineno">  483</span>&#160;</div>
<div class="line"><a name="l00484"></a><span class="lineno">  484</span>&#160;            rrtPath=<a class="code" href="classrrt__plan_1_1rrt__planner.html#a23e32ef1c189f5035a751047f84e052a">findCellPath</a>(<a class="code" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>, startCell, goalCell, gridVal);</div>
<div class="line"><a name="l00485"></a><span class="lineno">  485</span>&#160;            <span class="comment">//clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &amp;time3);</span></div>
<div class="line"><a name="l00486"></a><span class="lineno">  486</span>&#160;            t3=<a class="code" href="rrt__planner_8cpp.html#a3b34910310800875174c4846e64a61bd">getCurrentTime</a>();</div>
<div class="line"><a name="l00487"></a><span class="lineno">  487</span>&#160;</div>
<div class="line"><a name="l00488"></a><span class="lineno">  488</span>&#160;            <span class="comment">//cout&lt;&lt;&quot;improved RRT time=&quot;&lt;&lt;(diff(time1,time3).tv_sec)*1e3 + (diff(time1,time3).tv_nsec)*1e-6&lt;&lt;endl;</span></div>
<div class="line"><a name="l00489"></a><span class="lineno">  489</span>&#160;            <span class="comment">//irrtlog&lt;&lt;&quot;improved RRT time=&quot;&lt;&lt;(diff(time1,time3).tv_sec)*1e3 + (diff(time1,time3).tv_nsec)*1e-6&lt;&lt;endl;</span></div>
<div class="line"><a name="l00490"></a><span class="lineno">  490</span>&#160;</div>
<div class="line"><a name="l00491"></a><span class="lineno">  491</span>&#160;            <span class="comment">//float a=(diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6;</span></div>
<div class="line"><a name="l00492"></a><span class="lineno">  492</span>&#160;            <span class="comment">//float b=(diff(time1,time3).tv_sec)*1e3 + (diff(time1,time3).tv_nsec)*1e-6;</span></div>
<div class="line"><a name="l00493"></a><span class="lineno">  493</span>&#160;            <span class="comment">//float timeRatio=a/b;</span></div>
<div class="line"><a name="l00494"></a><span class="lineno">  494</span>&#160;</div>
<div class="line"><a name="l00495"></a><span class="lineno">  495</span>&#160;            <span class="comment">//cout&lt;&lt;&quot;temperature field time=&quot;&lt;&lt;a&lt;&lt;endl;</span></div>
<div class="line"><a name="l00496"></a><span class="lineno">  496</span>&#160;            <span class="comment">//cout&lt;&lt;&quot;timeRatio=&quot;&lt;&lt;timeRatio&lt;&lt;endl;</span></div>
<div class="line"><a name="l00497"></a><span class="lineno">  497</span>&#160;            </div>
<div class="line"><a name="l00498"></a><span class="lineno">  498</span>&#160;            cout&lt;&lt;<span class="stringliteral">&quot;temperature field time=&quot;</span>&lt;&lt;t2-t1&lt;&lt;endl;</div>
<div class="line"><a name="l00499"></a><span class="lineno">  499</span>&#160;            <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;temperature field time=&quot;</span>&lt;&lt;t2-t1&lt;&lt;endl;</div>
<div class="line"><a name="l00500"></a><span class="lineno">  500</span>&#160;            cout&lt;&lt;<span class="stringliteral">&quot;planning time=&quot;</span>&lt;&lt;t3-t2&lt;&lt;endl;</div>
<div class="line"><a name="l00501"></a><span class="lineno">  501</span>&#160;            <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;planning time=&quot;</span>&lt;&lt;t3-t2&lt;&lt;endl;</div>
<div class="line"><a name="l00502"></a><span class="lineno">  502</span>&#160;            <span class="comment">//cout&lt;&lt;&quot;total time=&quot;&lt;&lt;t3-t1&lt;&lt;endl;</span></div>
<div class="line"><a name="l00503"></a><span class="lineno">  503</span>&#160;    }</div>
<div class="line"><a name="l00504"></a><span class="lineno">  504</span>&#160;    <span class="keywordflow">else</span></div>
<div class="line"><a name="l00505"></a><span class="lineno">  505</span>&#160;    {</div>
<div class="line"><a name="l00506"></a><span class="lineno">  506</span>&#160;        ROS_WARN(<span class="stringliteral">&quot;------Not valid start or goal------&quot;</span>);</div>
<div class="line"><a name="l00507"></a><span class="lineno">  507</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
<div class="line"><a name="l00508"></a><span class="lineno">  508</span>&#160;    }</div>
<div class="line"><a name="l00509"></a><span class="lineno">  509</span>&#160;</div>
<div class="line"><a name="l00510"></a><span class="lineno">  510</span>&#160;    <span class="comment">//if the global planner find a path</span></div>
<div class="line"><a name="l00511"></a><span class="lineno">  511</span>&#160;    <span class="keywordflow">if</span> (rrtPath.size()&gt;0)</div>
<div class="line"><a name="l00512"></a><span class="lineno">  512</span>&#160;    {</div>
<div class="line"><a name="l00513"></a><span class="lineno">  513</span>&#160;        <a class="code" href="classrrt__plan_1_1rrt__planner.html#aac5e9f72b4c55ce4aada8d80eed92a9e">convertToPlan</a>(rrtPath,plan,goal,goalCell);</div>
<div class="line"><a name="l00514"></a><span class="lineno">  514</span>&#160;</div>
<div class="line"><a name="l00515"></a><span class="lineno">  515</span>&#160;        <span class="keywordtype">float</span> path_length = 0.0;</div>
<div class="line"><a name="l00516"></a><span class="lineno">  516</span>&#160;</div>
<div class="line"><a name="l00517"></a><span class="lineno">  517</span>&#160;        std::vector&lt;geometry_msgs::PoseStamped&gt;::iterator it = plan.begin();</div>
<div class="line"><a name="l00518"></a><span class="lineno">  518</span>&#160;</div>
<div class="line"><a name="l00519"></a><span class="lineno">  519</span>&#160;        geometry_msgs::PoseStamped last_pose;</div>
<div class="line"><a name="l00520"></a><span class="lineno">  520</span>&#160;        last_pose = *it;</div>
<div class="line"><a name="l00521"></a><span class="lineno">  521</span>&#160;        it++;</div>
<div class="line"><a name="l00522"></a><span class="lineno">  522</span>&#160;        <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;路径位姿点：&quot;</span>&lt;&lt;endl;</div>
<div class="line"><a name="l00523"></a><span class="lineno">  523</span>&#160;        <span class="keywordflow">for</span> (; it!=plan.end(); ++it)</div>
<div class="line"><a name="l00524"></a><span class="lineno">  524</span>&#160;        {</div>
<div class="line"><a name="l00525"></a><span class="lineno">  525</span>&#160;            <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;(&quot;</span>&lt;&lt;(*it).pose.position.x&lt;&lt;<span class="stringliteral">&quot;,&quot;</span>&lt;&lt;(*it).pose.position.y&lt;&lt;<span class="stringliteral">&quot;)&quot;</span>&lt;&lt;endl;</div>
<div class="line"><a name="l00526"></a><span class="lineno">  526</span>&#160;            path_length += hypot(  (*it).pose.position.x - last_pose.pose.position.x,</div>
<div class="line"><a name="l00527"></a><span class="lineno">  527</span>&#160;                                   (*it).pose.position.y - last_pose.pose.position.y );</div>
<div class="line"><a name="l00528"></a><span class="lineno">  528</span>&#160;            last_pose = *it;</div>
<div class="line"><a name="l00529"></a><span class="lineno">  529</span>&#160;        }</div>
<div class="line"><a name="l00530"></a><span class="lineno">  530</span>&#160;        cout &lt;&lt;<span class="stringliteral">&quot;------The global path length: &quot;</span>&lt;&lt; path_length&lt;&lt; <span class="stringliteral">&quot; meters------&quot;</span>&lt;&lt;endl;</div>
<div class="line"><a name="l00531"></a><span class="lineno">  531</span>&#160;        <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a> &lt;&lt;<span class="stringliteral">&quot;------The global path length: &quot;</span>&lt;&lt; path_length&lt;&lt; <span class="stringliteral">&quot; meters------&quot;</span>&lt;&lt;endl;</div>
<div class="line"><a name="l00532"></a><span class="lineno">  532</span>&#160;    }</div>
<div class="line"><a name="l00533"></a><span class="lineno">  533</span>&#160;    <span class="keywordflow">else</span></div>
<div class="line"><a name="l00534"></a><span class="lineno">  534</span>&#160;    {</div>
<div class="line"><a name="l00535"></a><span class="lineno">  535</span>&#160;        ROS_WARN(<span class="stringliteral">&quot;------The planner failed to find a path, choose other goal position------&quot;</span>);</div>
<div class="line"><a name="l00536"></a><span class="lineno">  536</span>&#160;        <span class="comment">//return false;</span></div>
<div class="line"><a name="l00537"></a><span class="lineno">  537</span>&#160;    }</div>
<div class="line"><a name="l00538"></a><span class="lineno">  538</span>&#160;</div>
<div class="line"><a name="l00539"></a><span class="lineno">  539</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#ac2fe5313cbc77dffddfcfe1f6605d53e">publishPlan</a>(plan);</div>
<div class="line"><a name="l00540"></a><span class="lineno">  540</span>&#160;    </div>
<div class="line"><a name="l00541"></a><span class="lineno">  541</span>&#160;     <span class="comment">/*</span></div>
<div class="line"><a name="l00542"></a><span class="lineno">  542</span>&#160;<span class="comment">     long t4,t5;</span></div>
<div class="line"><a name="l00543"></a><span class="lineno">  543</span>&#160;<span class="comment">     timespec time4, time5;</span></div>
<div class="line"><a name="l00544"></a><span class="lineno">  544</span>&#160;<span class="comment">     clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &amp;time4);</span></div>
<div class="line"><a name="l00545"></a><span class="lineno">  545</span>&#160;<span class="comment">     t4=getCurrentTime();</span></div>
<div class="line"><a name="l00546"></a><span class="lineno">  546</span>&#160;<span class="comment">     for(int i=0;i&lt;500;i++)</span></div>
<div class="line"><a name="l00547"></a><span class="lineno">  547</span>&#160;<span class="comment">     {</span></div>
<div class="line"><a name="l00548"></a><span class="lineno">  548</span>&#160;<span class="comment">         plan=middleoptimizationLayer(plan);</span></div>
<div class="line"><a name="l00549"></a><span class="lineno">  549</span>&#160;<span class="comment">     }</span></div>
<div class="line"><a name="l00550"></a><span class="lineno">  550</span>&#160;<span class="comment">     clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &amp;time5);</span></div>
<div class="line"><a name="l00551"></a><span class="lineno">  551</span>&#160;<span class="comment">     t5=getCurrentTime();</span></div>
<div class="line"><a name="l00552"></a><span class="lineno">  552</span>&#160;<span class="comment">     cout&lt;&lt;&quot;optimal time=&quot;&lt;&lt;t5-t4&lt;&lt;endl;</span></div>
<div class="line"><a name="l00553"></a><span class="lineno">  553</span>&#160;<span class="comment">     cout&lt;&lt;&quot;optimal time2=&quot;&lt;&lt;(diff(time4,time5).tv_sec)*1e3 + (diff(time4,time5).tv_nsec)*1e-6&lt;&lt;endl;</span></div>
<div class="line"><a name="l00554"></a><span class="lineno">  554</span>&#160;<span class="comment">     */</span></div>
<div class="line"><a name="l00555"></a><span class="lineno">  555</span>&#160;     </div>
<div class="line"><a name="l00556"></a><span class="lineno">  556</span>&#160;     <span class="comment">/*</span></div>
<div class="line"><a name="l00557"></a><span class="lineno">  557</span>&#160;<span class="comment">     float path_length = 0.0;</span></div>
<div class="line"><a name="l00558"></a><span class="lineno">  558</span>&#160;<span class="comment">     std::vector&lt;geometry_msgs::PoseStamped&gt;::iterator it = plan.begin();</span></div>
<div class="line"><a name="l00559"></a><span class="lineno">  559</span>&#160;<span class="comment">     geometry_msgs::PoseStamped last_pose;</span></div>
<div class="line"><a name="l00560"></a><span class="lineno">  560</span>&#160;<span class="comment">     last_pose = *it;</span></div>
<div class="line"><a name="l00561"></a><span class="lineno">  561</span>&#160;<span class="comment">     it++;</span></div>
<div class="line"><a name="l00562"></a><span class="lineno">  562</span>&#160;<span class="comment">     irrtlog&lt;&lt;&quot;优化后路径位姿点：&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00563"></a><span class="lineno">  563</span>&#160;<span class="comment">     for (; it!=plan.end(); ++it)</span></div>
<div class="line"><a name="l00564"></a><span class="lineno">  564</span>&#160;<span class="comment">     {</span></div>
<div class="line"><a name="l00565"></a><span class="lineno">  565</span>&#160;<span class="comment">            irrtlog&lt;&lt;&quot;(&quot;&lt;&lt;(*it).pose.position.x&lt;&lt;&quot;,&quot;&lt;&lt;(*it).pose.position.y&lt;&lt;&quot;)&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00566"></a><span class="lineno">  566</span>&#160;<span class="comment">            path_length += hypot(  (*it).pose.position.x - last_pose.pose.position.x,</span></div>
<div class="line"><a name="l00567"></a><span class="lineno">  567</span>&#160;<span class="comment">                                   (*it).pose.position.y - last_pose.pose.position.y );</span></div>
<div class="line"><a name="l00568"></a><span class="lineno">  568</span>&#160;<span class="comment">            last_pose = *it;</span></div>
<div class="line"><a name="l00569"></a><span class="lineno">  569</span>&#160;<span class="comment">     }</span></div>
<div class="line"><a name="l00570"></a><span class="lineno">  570</span>&#160;<span class="comment">     cout &lt;&lt;&quot;------The  optimized global path length: &quot;&lt;&lt; path_length&lt;&lt; &quot; meters------&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00571"></a><span class="lineno">  571</span>&#160;<span class="comment">     irrtlog &lt;&lt;&quot;------The  optimized global path length: &quot;&lt;&lt; path_length&lt;&lt; &quot; meters------&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00572"></a><span class="lineno">  572</span>&#160;<span class="comment">     */</span></div>
<div class="line"><a name="l00573"></a><span class="lineno">  573</span>&#160;    </div>
<div class="line"><a name="l00574"></a><span class="lineno">  574</span>&#160;     <span class="comment">//publishPlan2(plan);</span></div>
<div class="line"><a name="l00575"></a><span class="lineno">  575</span>&#160;</div>
<div class="line"><a name="l00576"></a><span class="lineno">  576</span>&#160;</div>
<div class="line"><a name="l00577"></a><span class="lineno">  577</span>&#160;    <span class="comment">//cout&lt;&lt;&quot;+++++++++++++++++++++++++++++++&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00578"></a><span class="lineno">  578</span>&#160;    <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;+++++++++++++++++++++++++++++++&quot;</span>&lt;&lt;endl;</div>
<div class="line"><a name="l00579"></a><span class="lineno">  579</span>&#160;    <span class="comment">//cout&lt;&lt;&quot;树中所有结点(格式：结点ID，该结点的父结点ID，x坐标值，y坐标值):&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00580"></a><span class="lineno">  580</span>&#160;    <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;树中所有结点(格式：结点ID，该结点的父结点ID，x坐标值，y坐标值):&quot;</span>&lt;&lt;endl;</div>
<div class="line"><a name="l00581"></a><span class="lineno">  581</span>&#160;    <a class="code" href="structRRT_1_1rrtNode.html">RRT::rrtNode</a>  l;</div>
<div class="line"><a name="l00582"></a><span class="lineno">  582</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> m=0; m&lt;<a class="code" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>.<a class="code" href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">getTreeSize</a>(); m++)</div>
<div class="line"><a name="l00583"></a><span class="lineno">  583</span>&#160;    {</div>
<div class="line"><a name="l00584"></a><span class="lineno">  584</span>&#160;        l=<a class="code" href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a>.<a class="code" href="classRRT.html#afde36442771a9552bb97572098396c2b">getNode</a>(m);</div>
<div class="line"><a name="l00585"></a><span class="lineno">  585</span>&#160;        <span class="comment">//cout&lt;&lt;l.nodeID&lt;&lt;&quot;,&quot;&lt;&lt;l.parentID&lt;&lt;&quot;,&quot;&lt;&lt;l.posX* resolution+originX&lt;&lt;&quot;,&quot;&lt;&lt;l.posY* resolution+originY&lt;&lt;endl;</span></div>
<div class="line"><a name="l00586"></a><span class="lineno">  586</span>&#160;        <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;l.<a class="code" href="structRRT_1_1rrtNode.html#aa8adf5b50e1a2a466771ae3af56b1658">nodeID</a>&lt;&lt;<span class="stringliteral">&quot;,&quot;</span>&lt;&lt;l.<a class="code" href="structRRT_1_1rrtNode.html#a9f5b9bfca61e25fe7f80d361ed7996ff">parentID</a>&lt;&lt;<span class="stringliteral">&quot;,&quot;</span>&lt;&lt;l.<a class="code" href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">posX</a>* <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>+<a class="code" href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">originX</a>&lt;&lt;<span class="stringliteral">&quot;,&quot;</span>&lt;&lt;l.<a class="code" href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">posY</a>* <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>+<a class="code" href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">originY</a>&lt;&lt;endl;</div>
<div class="line"><a name="l00587"></a><span class="lineno">  587</span>&#160;    }</div>
<div class="line"><a name="l00588"></a><span class="lineno">  588</span>&#160;    <span class="comment">//cout&lt;&lt;&quot;+++++++++++++++++++++++++++++++&quot;&lt;&lt;endl;</span></div>
<div class="line"><a name="l00589"></a><span class="lineno">  589</span>&#160;    <a class="code" href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a>&lt;&lt;<span class="stringliteral">&quot;+++++++++++++++++++++++++++++++&quot;</span>&lt;&lt;endl;</div>
<div class="line"><a name="l00590"></a><span class="lineno">  590</span>&#160;</div>
<div class="line"><a name="l00591"></a><span class="lineno">  591</span>&#160;    <span class="keywordflow">return</span> !plan.empty();</div>
<div class="line"><a name="l00592"></a><span class="lineno">  592</span>&#160;}</div>
<div class="ttc" id="rrt_8h_html_afb8b6147953aaf9e7089f5f157d5b020"><div class="ttname"><a href="rrt_8h.html#afb8b6147953aaf9e7089f5f157d5b020">mapReduceSize</a></div><div class="ttdeci">int mapReduceSize</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00023">rrt.h:23</a></div></div>
<div class="ttc" id="rrt__planner_8cpp_html_a51725e16375c08b52f82eb2faa5a921c"><div class="ttname"><a href="rrt__planner_8cpp.html#a51725e16375c08b52f82eb2faa5a921c">myRRT</a></div><div class="ttdeci">RRT myRRT</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00029">rrt_planner.cpp:29</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a9f5b9bfca61e25fe7f80d361ed7996ff"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a9f5b9bfca61e25fe7f80d361ed7996ff">RRT::rrtNode::parentID</a></div><div class="ttdeci">int parentID</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00040">rrt.h:40</a></div></div>
<div class="ttc" id="rrt__planner_8cpp_html_a3b34910310800875174c4846e64a61bd"><div class="ttname"><a href="rrt__planner_8cpp.html#a3b34910310800875174c4846e64a61bd">getCurrentTime</a></div><div class="ttdeci">long getCurrentTime()</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00034">rrt_planner.cpp:34</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3dcf8908136ace532176511eb1861528"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">rrt_plan::rrt_planner::costmap_ros_</a></div><div class="ttdeci">costmap_2d::Costmap2DROS * costmap_ros_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00337">inspire_rrt_planner.h:337</a></div></div>
<div class="ttc" id="classRRT_html_afde36442771a9552bb97572098396c2b"><div class="ttname"><a href="classRRT.html#afde36442771a9552bb97572098396c2b">RRT::getNode</a></div><div class="ttdeci">rrtNode getNode(int nodeID)</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00159">rrt.h:159</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a5cfa182b92acb4e370ba19a213a0c22a"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a5cfa182b92acb4e370ba19a213a0c22a">rrt_plan::rrt_planner::originY</a></div><div class="ttdeci">float originY</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00335">inspire_rrt_planner.h:335</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac74fe436b5e1fa30bda1fbe821369aa2"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac74fe436b5e1fa30bda1fbe821369aa2">rrt_plan::rrt_planner::width</a></div><div class="ttdeci">int width</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00342">inspire_rrt_planner.h:342</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a2647265d06a792b983b372af52cf5677"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a2647265d06a792b983b372af52cf5677">rrt_plan::rrt_planner::convertToCellIndex</a></div><div class="ttdeci">int convertToCellIndex(float x, float y)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00135">inspire_rrt_planner.h:135</a></div></div>
<div class="ttc" id="classRRT_html_a05dc852e0b777042e290dcc96bf605bb"><div class="ttname"><a href="classRRT.html#a05dc852e0b777042e290dcc96bf605bb">RRT::getTreeSize</a></div><div class="ttdeci">int getTreeSize()</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00130">rrt.h:130</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac2fe5313cbc77dffddfcfe1f6605d53e"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac2fe5313cbc77dffddfcfe1f6605d53e">rrt_plan::rrt_planner::publishPlan</a></div><div class="ttdeci">void publishPlan(const std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;path)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00595">rrt_planner.cpp:595</a></div></div>
<div class="ttc" id="classRRT_html_affd1f4f4e35541cdca748bac5c1bbd34"><div class="ttname"><a href="classRRT.html#affd1f4f4e35541cdca748bac5c1bbd34">RRT::clearRRT</a></div><div class="ttdeci">void clearRRT()</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00297">rrt.h:297</a></div></div>
<div class="ttc" id="rrt__planner_8cpp_html_ae63d6003ff9416f7c0be6892e5bb899c"><div class="ttname"><a href="rrt__planner_8cpp.html#ae63d6003ff9416f7c0be6892e5bb899c">irrtlog</a></div><div class="ttdeci">ofstream irrtlog(&quot;/home/xuefengchang/irrtlog.txt&quot;)</div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_aa8adf5b50e1a2a466771ae3af56b1658"><div class="ttname"><a href="structRRT_1_1rrtNode.html#aa8adf5b50e1a2a466771ae3af56b1658">RRT::rrtNode::nodeID</a></div><div class="ttdeci">int nodeID</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00037">rrt.h:37</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a219cf78a0f5689bc84744df52c37e84d"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a219cf78a0f5689bc84744df52c37e84d">RRT::rrtNode::posY</a></div><div class="ttdeci">float posY</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00039">rrt.h:39</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ad23ca65f53176297ae7e9f40c7edeb69"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">rrt_plan::rrt_planner::resolution</a></div><div class="ttdeci">float resolution</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00336">inspire_rrt_planner.h:336</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a92ba133c58a9ad0de8f425e856953ac1"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a92ba133c58a9ad0de8f425e856953ac1">rrt_plan::rrt_planner::originX</a></div><div class="ttdeci">float originX</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00334">inspire_rrt_planner.h:334</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac1cd8655573adac056ae922a92337b8d"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">rrt_plan::rrt_planner::initialized_</a></div><div class="ttdeci">bool initialized_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00341">inspire_rrt_planner.h:341</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html"><div class="ttname"><a href="structRRT_1_1rrtNode.html">RRT::rrtNode</a></div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00035">rrt.h:35</a></div></div>
<div class="ttc" id="structRRT_1_1rrtNode_html_a4c2096fa1d719cf2aa49cf21916a5a4b"><div class="ttname"><a href="structRRT_1_1rrtNode.html#a4c2096fa1d719cf2aa49cf21916a5a4b">RRT::rrtNode::posX</a></div><div class="ttdeci">float posX</div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00038">rrt.h:38</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a758810b2f257fe0778ed3eb1e970d99c"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a758810b2f257fe0778ed3eb1e970d99c">rrt_plan::rrt_planner::getCellColID</a></div><div class="ttdeci">int getCellColID(int index)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00130">inspire_rrt_planner.h:130</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a8f1fde39d2f3b01d7b2d11dc9482e1dc"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a8f1fde39d2f3b01d7b2d11dc9482e1dc">rrt_plan::rrt_planner::getCorrdinate</a></div><div class="ttdeci">void getCorrdinate(float &amp;x, float &amp;y)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00114">inspire_rrt_planner.h:114</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3549de56a50bff6494514878b64584df"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3549de56a50bff6494514878b64584df">rrt_plan::rrt_planner::height</a></div><div class="ttdeci">int height</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00343">inspire_rrt_planner.h:343</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_aff6d98168c072b176287fc9c444349c6"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#aff6d98168c072b176287fc9c444349c6">rrt_plan::rrt_planner::getGridVal</a></div><div class="ttdeci">void getGridVal(vector&lt; vector&lt; float &gt; &gt; &amp;gridVal, int startRowID, int startColID, int goalRowID, int goalColID)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00240">rrt_planner.cpp:240</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_aa43a4ec62844e679d9e0aefcbfa5f286"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#aa43a4ec62844e679d9e0aefcbfa5f286">rrt_plan::rrt_planner::isStartAndGoalCellsValid</a></div><div class="ttdeci">bool isStartAndGoalCellsValid(int startCell, int goalCell)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00833">rrt_planner.cpp:833</a></div></div>
<div class="ttc" id="classRRT_html"><div class="ttname"><a href="classRRT.html">RRT</a></div><div class="ttdef"><b>Definition:</b> <a href="rrt_8h_source.html#l00027">rrt.h:27</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a23e32ef1c189f5035a751047f84e052a"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a23e32ef1c189f5035a751047f84e052a">rrt_plan::rrt_planner::findCellPath</a></div><div class="ttdeci">vector&lt; int &gt; findCellPath(RRT &amp;myRRT, int startCell, int goalCell, vector&lt; vector&lt; float &gt; &gt; gridVal)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00651">rrt_planner.cpp:651</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_aa3c07d6b2b6bed62e4b60d3898fc80cf"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#aa3c07d6b2b6bed62e4b60d3898fc80cf">rrt_plan::rrt_planner::getCellRowID</a></div><div class="ttdeci">int getCellRowID(int index)</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00125">inspire_rrt_planner.h:125</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a07347499c296c693631f24dfb1cb4196"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a07347499c296c693631f24dfb1cb4196">rrt_plan::rrt_planner::isCellInsideMap</a></div><div class="ttdeci">bool isCellInsideMap(float x, float y)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00778">rrt_planner.cpp:778</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_aac5e9f72b4c55ce4aada8d80eed92a9e"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#aac5e9f72b4c55ce4aada8d80eed92a9e">rrt_plan::rrt_planner::convertToPlan</a></div><div class="ttdeci">void convertToPlan(vector&lt; int &gt; bestPath, std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;plan, geometry_msgs::PoseStamped goal, int goalCell)</div><div class="ttdef"><b>Definition:</b> <a href="rrt__planner_8cpp_source.html#l00692">rrt_planner.cpp:692</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a7cbacb0aead733fc6cfde463c6ea2812"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::mapToWorld </td>
          <td>(</td>
          <td class="paramtype">double&#160;</td>
          <td class="paramname"><em>mx</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">double&#160;</td>
          <td class="paramname"><em>my</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">double &amp;&#160;</td>
          <td class="paramname"><em>wx</em>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">double &amp;&#160;</td>
          <td class="paramname"><em>wy</em>&#160;</td>
        </tr>
        <tr>
          <td></td>
          <td>)</td>
          <td></td><td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00788">788</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00789"></a><span class="lineno">  789</span>&#160;{</div>
<div class="line"><a name="l00790"></a><span class="lineno">  790</span>&#160;    costmap_2d::Costmap2D* costmap = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">costmap_ros_</a>-&gt;getCostmap();</div>
<div class="line"><a name="l00791"></a><span class="lineno">  791</span>&#160;    wx = costmap-&gt;getOriginX() + mx * <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>;</div>
<div class="line"><a name="l00792"></a><span class="lineno">  792</span>&#160;    wy = costmap-&gt;getOriginY() + my * <a class="code" href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">resolution</a>;</div>
<div class="line"><a name="l00793"></a><span class="lineno">  793</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3dcf8908136ace532176511eb1861528"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">rrt_plan::rrt_planner::costmap_ros_</a></div><div class="ttdeci">costmap_2d::Costmap2DROS * costmap_ros_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00337">inspire_rrt_planner.h:337</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ad23ca65f53176297ae7e9f40c7edeb69"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ad23ca65f53176297ae7e9f40c7edeb69">rrt_plan::rrt_planner::resolution</a></div><div class="ttdeci">float resolution</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00336">inspire_rrt_planner.h:336</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="aac168be892772fe17a1d841d87c2249c"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">std::vector&lt; geometry_msgs::PoseStamped &gt; rrt_plan::rrt_planner::middleoptimizationLayer </td>
          <td>(</td>
          <td class="paramtype">std::vector&lt; geometry_msgs::PoseStamped &gt;&#160;</td>
          <td class="paramname"><em>temp_transformed_plan</em></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00321">321</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;{</div>
<div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;    <span class="comment">//std::cout&lt;&lt;&quot;初始局部路径长度：&quot;&lt;&lt;temp_transformed_plan.size()&lt;&lt;&quot;\n&quot;;</span></div>
<div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;</div>
<div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;    std::vector&lt;geometry_msgs::PoseStamped&gt; transformed_plan;</div>
<div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;    </div>
<div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;temp_transformed_plan.size(); i++)</div>
<div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;    {</div>
<div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;        transformed_plan.push_back(temp_transformed_plan[i]);</div>
<div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;    }</div>
<div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;</div>
<div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;    geometry_msgs::PoseStamped mid_pose;</div>
<div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;    geometry_msgs::PoseStamped pre_pose;</div>
<div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;    geometry_msgs::PoseStamped beh_pose;</div>
<div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;</div>
<div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;    costmap_2d::Costmap2D* costmap = <a class="code" href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">costmap_ros_</a>-&gt;getCostmap();</div>
<div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;</div>
<div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;    <span class="keywordflow">if</span>((transformed_plan.size())&gt;=3)  <span class="comment">//当局部路径大于等于两个划分(每个划分3个结点)时进入中间优化层</span></div>
<div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;    {</div>
<div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j=1; j&lt;=transformed_plan.size()-2; j++)</div>
<div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;        {</div>
<div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;                   pre_pose=transformed_plan[j-1];</div>
<div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;                   beh_pose=transformed_plan[j+1];</div>
<div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160;                   </div>
<div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;                   mid_pose.pose.position.x=(pre_pose.pose.position.x+beh_pose.pose.position.x)/2.0;</div>
<div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;                   mid_pose.pose.position.y=(pre_pose.pose.position.y+beh_pose.pose.position.y)/2.0;</div>
<div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;                   mid_pose.pose.position.z=0.0;</div>
<div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160;                  </div>
<div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;                   mid_pose.pose.orientation.x=0.0;</div>
<div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;                   mid_pose.pose.orientation.y=0.0;</div>
<div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160;                   mid_pose.pose.orientation.z=0.0;</div>
<div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160;                   mid_pose.pose.orientation.w=1.0;</div>
<div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;</div>
<div class="line"><a name="l00354"></a><span class="lineno">  354</span>&#160;                   <span class="comment">//ROS_INFO(&quot;mid_pose.pose.position.x=%f, mid_pose.pose.position.y=%f&quot;, mid_pose.pose.position.x, mid_pose.pose.position.y);</span></div>
<div class="line"><a name="l00355"></a><span class="lineno">  355</span>&#160;</div>
<div class="line"><a name="l00356"></a><span class="lineno">  356</span>&#160;                   <span class="keywordtype">int</span> mid_pose_x=(int)((mid_pose.pose.position.x-costmap-&gt;getOriginX())/costmap-&gt;getResolution());</div>
<div class="line"><a name="l00357"></a><span class="lineno">  357</span>&#160;                   <span class="keywordtype">int</span> mid_pose_y=(int)((mid_pose.pose.position.y-costmap-&gt;getOriginY())/costmap-&gt;getResolution());</div>
<div class="line"><a name="l00358"></a><span class="lineno">  358</span>&#160;                   </div>
<div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;                   <span class="keywordtype">bool</span> x_flag=(mid_pose_x&lt;costmap-&gt;getSizeInCellsX())&amp;&amp;(mid_pose_x&gt;=0);</div>
<div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;                   <span class="keywordtype">bool</span> y_flag=(mid_pose_y&lt;costmap-&gt;getSizeInCellsY())&amp;&amp;(mid_pose_y&gt;=0);</div>
<div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;                   </div>
<div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;                   <span class="comment">//if((mid_pose_x&lt;costmap-&gt;getSizeInCellsX())&amp;&amp;(mid_pose_y&lt;costmap-&gt;getSizeInCellsY()))</span></div>
<div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;                   <span class="keywordflow">if</span>(x_flag&amp;&amp;y_flag)</div>
<div class="line"><a name="l00364"></a><span class="lineno">  364</span>&#160;                   {</div>
<div class="line"><a name="l00365"></a><span class="lineno">  365</span>&#160;                         <span class="keywordtype">int</span> cost=costmap-&gt;getCost(mid_pose_x, mid_pose_y);</div>
<div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;</div>
<div class="line"><a name="l00367"></a><span class="lineno">  367</span>&#160;                         <span class="keywordflow">if</span>(cost==0)<span class="comment">//free space</span></div>
<div class="line"><a name="l00368"></a><span class="lineno">  368</span>&#160;                         <span class="comment">//if(cost&lt;=127)//free space</span></div>
<div class="line"><a name="l00369"></a><span class="lineno">  369</span>&#160;                         {</div>
<div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;                             transformed_plan[j].pose.position.x=mid_pose.pose.position.x;</div>
<div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;                             transformed_plan[j].pose.position.y=mid_pose.pose.position.y;</div>
<div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;                         }</div>
<div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;                       </div>
<div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160;                   }</div>
<div class="line"><a name="l00375"></a><span class="lineno">  375</span>&#160;            }</div>
<div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;     }</div>
<div class="line"><a name="l00377"></a><span class="lineno">  377</span>&#160;    <span class="comment">//std::cout&lt;&lt;&quot;优化后局部路径长度：&quot;&lt;&lt;transformed_plan.size()&lt;&lt;&quot;\n&quot;;</span></div>
<div class="line"><a name="l00378"></a><span class="lineno">  378</span>&#160;    <span class="keywordflow">return</span>  transformed_plan;</div>
<div class="line"><a name="l00379"></a><span class="lineno">  379</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a3dcf8908136ace532176511eb1861528"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a3dcf8908136ace532176511eb1861528">rrt_plan::rrt_planner::costmap_ros_</a></div><div class="ttdeci">costmap_2d::Costmap2DROS * costmap_ros_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00337">inspire_rrt_planner.h:337</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="ac2fe5313cbc77dffddfcfe1f6605d53e"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::publishPlan </td>
          <td>(</td>
          <td class="paramtype">const std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;&#160;</td>
          <td class="paramname"><em>path</em></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00595">595</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00596"></a><span class="lineno">  596</span>&#160;{</div>
<div class="line"><a name="l00597"></a><span class="lineno">  597</span>&#160;    <span class="keywordflow">if</span> (!<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">initialized_</a>)</div>
<div class="line"><a name="l00598"></a><span class="lineno">  598</span>&#160;    {</div>
<div class="line"><a name="l00599"></a><span class="lineno">  599</span>&#160;        ROS_ERROR(</div>
<div class="line"><a name="l00600"></a><span class="lineno">  600</span>&#160;            <span class="stringliteral">&quot;This planner has not been initialized yet, but it is being used, please call initialize() before use&quot;</span>);</div>
<div class="line"><a name="l00601"></a><span class="lineno">  601</span>&#160;        <span class="keywordflow">return</span>;</div>
<div class="line"><a name="l00602"></a><span class="lineno">  602</span>&#160;    }</div>
<div class="line"><a name="l00603"></a><span class="lineno">  603</span>&#160;</div>
<div class="line"><a name="l00604"></a><span class="lineno">  604</span>&#160;    <span class="comment">//create a message for the plan</span></div>
<div class="line"><a name="l00605"></a><span class="lineno">  605</span>&#160;    nav_msgs::Path gui_path;</div>
<div class="line"><a name="l00606"></a><span class="lineno">  606</span>&#160;    gui_path.poses.resize(path.size());</div>
<div class="line"><a name="l00607"></a><span class="lineno">  607</span>&#160;</div>
<div class="line"><a name="l00608"></a><span class="lineno">  608</span>&#160;    <span class="keywordflow">if</span> (!path.empty())</div>
<div class="line"><a name="l00609"></a><span class="lineno">  609</span>&#160;    {</div>
<div class="line"><a name="l00610"></a><span class="lineno">  610</span>&#160;        gui_path.header.frame_id = path[0].header.frame_id;</div>
<div class="line"><a name="l00611"></a><span class="lineno">  611</span>&#160;        gui_path.header.stamp = path[0].header.stamp;</div>
<div class="line"><a name="l00612"></a><span class="lineno">  612</span>&#160;    }</div>
<div class="line"><a name="l00613"></a><span class="lineno">  613</span>&#160;</div>
<div class="line"><a name="l00614"></a><span class="lineno">  614</span>&#160;    <span class="comment">// Extract the plan in world co-ordinates, we assume the path is all in the same frame</span></div>
<div class="line"><a name="l00615"></a><span class="lineno">  615</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0; i &lt; path.size(); i++)</div>
<div class="line"><a name="l00616"></a><span class="lineno">  616</span>&#160;    {</div>
<div class="line"><a name="l00617"></a><span class="lineno">  617</span>&#160;        gui_path.poses[i] = path[i];</div>
<div class="line"><a name="l00618"></a><span class="lineno">  618</span>&#160;    }</div>
<div class="line"><a name="l00619"></a><span class="lineno">  619</span>&#160;</div>
<div class="line"><a name="l00620"></a><span class="lineno">  620</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#a6beed3a62613c7e24813416185d12929">plan_pub_</a>.publish(gui_path);</div>
<div class="line"><a name="l00621"></a><span class="lineno">  621</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a6beed3a62613c7e24813416185d12929"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a6beed3a62613c7e24813416185d12929">rrt_plan::rrt_planner::plan_pub_</a></div><div class="ttdeci">ros::Publisher plan_pub_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00345">inspire_rrt_planner.h:345</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac1cd8655573adac056ae922a92337b8d"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">rrt_plan::rrt_planner::initialized_</a></div><div class="ttdeci">bool initialized_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00341">inspire_rrt_planner.h:341</a></div></div>
</div><!-- fragment -->
</div>
</div>
<a class="anchor" id="a3071190151c4463f2e39f29c02c1f55d"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void rrt_plan::rrt_planner::publishPlan2 </td>
          <td>(</td>
          <td class="paramtype">const std::vector&lt; geometry_msgs::PoseStamped &gt; &amp;&#160;</td>
          <td class="paramname"><em>path</em></td><td>)</td>
          <td></td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a> 第 <a class="el" href="rrt__planner_8cpp_source.html#l00623">623</a> 行定义.</p>
<div class="fragment"><div class="line"><a name="l00624"></a><span class="lineno">  624</span>&#160;{</div>
<div class="line"><a name="l00625"></a><span class="lineno">  625</span>&#160;    <span class="keywordflow">if</span> (!<a class="code" href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">initialized_</a>)</div>
<div class="line"><a name="l00626"></a><span class="lineno">  626</span>&#160;    {</div>
<div class="line"><a name="l00627"></a><span class="lineno">  627</span>&#160;        ROS_ERROR(</div>
<div class="line"><a name="l00628"></a><span class="lineno">  628</span>&#160;            <span class="stringliteral">&quot;This planner has not been initialized yet, but it is being used, please call initialize() before use&quot;</span>);</div>
<div class="line"><a name="l00629"></a><span class="lineno">  629</span>&#160;        <span class="keywordflow">return</span>;</div>
<div class="line"><a name="l00630"></a><span class="lineno">  630</span>&#160;    }</div>
<div class="line"><a name="l00631"></a><span class="lineno">  631</span>&#160;</div>
<div class="line"><a name="l00632"></a><span class="lineno">  632</span>&#160;    <span class="comment">//create a message for the plan</span></div>
<div class="line"><a name="l00633"></a><span class="lineno">  633</span>&#160;    nav_msgs::Path gui_path;</div>
<div class="line"><a name="l00634"></a><span class="lineno">  634</span>&#160;    gui_path.poses.resize(path.size());</div>
<div class="line"><a name="l00635"></a><span class="lineno">  635</span>&#160;</div>
<div class="line"><a name="l00636"></a><span class="lineno">  636</span>&#160;    <span class="keywordflow">if</span> (!path.empty())</div>
<div class="line"><a name="l00637"></a><span class="lineno">  637</span>&#160;    {</div>
<div class="line"><a name="l00638"></a><span class="lineno">  638</span>&#160;        gui_path.header.frame_id = path[0].header.frame_id;</div>
<div class="line"><a name="l00639"></a><span class="lineno">  639</span>&#160;        gui_path.header.stamp = path[0].header.stamp;</div>
<div class="line"><a name="l00640"></a><span class="lineno">  640</span>&#160;    }</div>
<div class="line"><a name="l00641"></a><span class="lineno">  641</span>&#160;</div>
<div class="line"><a name="l00642"></a><span class="lineno">  642</span>&#160;    <span class="comment">// Extract the plan in world co-ordinates, we assume the path is all in the same frame</span></div>
<div class="line"><a name="l00643"></a><span class="lineno">  643</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0; i &lt; path.size(); i++)</div>
<div class="line"><a name="l00644"></a><span class="lineno">  644</span>&#160;    {</div>
<div class="line"><a name="l00645"></a><span class="lineno">  645</span>&#160;        gui_path.poses[i] = path[i];</div>
<div class="line"><a name="l00646"></a><span class="lineno">  646</span>&#160;    }</div>
<div class="line"><a name="l00647"></a><span class="lineno">  647</span>&#160;</div>
<div class="line"><a name="l00648"></a><span class="lineno">  648</span>&#160;    <a class="code" href="classrrt__plan_1_1rrt__planner.html#a142a2d4c28bcaa2798493accf32690ee">plan_pub_2</a>.publish(gui_path);</div>
<div class="line"><a name="l00649"></a><span class="lineno">  649</span>&#160;}</div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_ac1cd8655573adac056ae922a92337b8d"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#ac1cd8655573adac056ae922a92337b8d">rrt_plan::rrt_planner::initialized_</a></div><div class="ttdeci">bool initialized_</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00341">inspire_rrt_planner.h:341</a></div></div>
<div class="ttc" id="classrrt__plan_1_1rrt__planner_html_a142a2d4c28bcaa2798493accf32690ee"><div class="ttname"><a href="classrrt__plan_1_1rrt__planner.html#a142a2d4c28bcaa2798493accf32690ee">rrt_plan::rrt_planner::plan_pub_2</a></div><div class="ttdeci">ros::Publisher plan_pub_2</div><div class="ttdef"><b>Definition:</b> <a href="inspire__rrt__planner_8h_source.html#l00346">inspire_rrt_planner.h:346</a></div></div>
</div><!-- fragment -->
</div>
</div>
<h2 class="groupheader">类成员变量说明</h2>
<a class="anchor" id="ad6f48786945a4a394c95de25cd4c77d2"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">costmap_2d::Costmap2D* rrt_plan::rrt_planner::costmap_</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00339">339</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="a3dcf8908136ace532176511eb1861528"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">costmap_2d::Costmap2DROS* rrt_plan::rrt_planner::costmap_ros_</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00337">337</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="a3549de56a50bff6494514878b64584df"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">int rrt_plan::rrt_planner::height</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00343">343</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="ac1cd8655573adac056ae922a92337b8d"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">bool rrt_plan::rrt_planner::initialized_</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00341">341</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="aae70d5e90c66f442c8822b573645e346"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">double rrt_plan::rrt_planner::min_dist_from_robot_</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00338">338</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="a92ba133c58a9ad0de8f425e856953ac1"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">float rrt_plan::rrt_planner::originX</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00334">334</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="a5cfa182b92acb4e370ba19a213a0c22a"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">float rrt_plan::rrt_planner::originY</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00335">335</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="a6beed3a62613c7e24813416185d12929"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">ros::Publisher rrt_plan::rrt_planner::plan_pub_</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00345">345</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="a142a2d4c28bcaa2798493accf32690ee"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">ros::Publisher rrt_plan::rrt_planner::plan_pub_2</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00346">346</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="ad23ca65f53176297ae7e9f40c7edeb69"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">float rrt_plan::rrt_planner::resolution</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00336">336</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="a9547688a6e31166034dc562a0b4608b6"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">ros::NodeHandle rrt_plan::rrt_planner::ROSNodeHandle</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00084">84</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="a25be58fd52b495ade819f8d5785beeae"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">double rrt_plan::rrt_planner::step_size_</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00338">338</a> 行定义.</p>

</div>
</div>
<a class="anchor" id="ac74fe436b5e1fa30bda1fbe821369aa2"></a>
<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">int rrt_plan::rrt_planner::width</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>在文件 <a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a> 第 <a class="el" href="inspire__rrt__planner_8h_source.html#l00342">342</a> 行定义.</p>

</div>
</div>
<hr/>该类的文档由以下文件生成:<ul>
<li>src/<a class="el" href="inspire__rrt__planner_8h_source.html">inspire_rrt_planner.h</a></li>
<li>src/<a class="el" href="rrt__planner_8cpp_source.html">rrt_planner.cpp</a></li>
</ul>
</div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
生成于 2017年 四月 7日 星期五 09:19:14 , 为 My Project使用  &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.6
</small></address>
</body>
</html>
